perm filename INTERP.PAS[AL,HE]5 blob
sn#697206 filedate 1983-01-28 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00055 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00006 00002 (*$E+ Routines to interpret an AL program *)
C00012 00003 (* datatype definitions *)
C00015 00004 (* statement definitions *)
C00019 00005 (* auxiliary definitions: variable, etc. *)
C00021 00006 (* definition of the ubiquitous NODE record *)
C00027 00007 (* records for parser: ident, token, resword *)
C00030 00008 (* process descriptor blocks & environment record definitions *)
C00034 00009 (* definition of AL-ARM messages *)
C00036 00010 (* global variables *)
C00038 00011 (* external routines *)
C00045 00012 (* message passing routines: sendCmd, sendTrans, getReply, whereArm *)
C00049 00013 (* aux routines: push, pop, upTrans, getELev, getEntry, getVar, gtVarn, getVal, setVal, getNval *)
C00058 00014 (* aux routines: getPdb, freePdb, getEvent, freeEvent *)
C00061 00015 (* graph structure routines: nextTime, getFrame, getDevice, feval, eval, change, invalidate, stvals ... *)
C00073 00016 (* aux routines to create & destroy variables: enterEntry,makeCmon,makeVar,killVar,killEnv,killNode,killStack *)
C00085 00017 (* aux io routines: prntSval, prntVec, prntTrans, prntStrng, prntPlist, onum, prntVar, badjoints *)
C00090 00018 (* aux routines: addPdb, sleep, deClkQueue, msgDispatch, swap *)
C00102 00019 (* aux routines: calibrate,initArms,initWorld,consDef,passConstants,flushLevel,flushAll,unwind,flushPdb,flushKids *)
C00121 00020 (* aux routines: cmonEnable, cmonDisable, cmonCheck *)
C00127 00021 (* expression evaluator: evalExp *)
C00145 00022 procedure doProg (* ** ** *)
C00146 00023 procedure doBlock
C00148 00024 procedure doCoblock
C00151 00025 procedure doEnd
C00155 00026 procedure doFor
C00158 00027 procedure doIf
C00159 00028 procedure doWhile
C00160 00029 procedure doUntil
C00161 00030 procedure doCase
C00163 00031 procedure doCall
C00164 00032 procedure doReturn
C00168 00033 procedure doPrint
C00169 00034 procedure doPrompt
C00171 00035 procedure doPause
C00172 00036 procedure doAbort
C00174 00037 procedure doAssign
C00176 00038 procedure doSignal
C00178 00039 procedure doWait
C00180 00040 procedure doEnable
C00181 00041 procedure doDisable
C00182 00042 (* affixment auxiliary routines: affixaux, unfixaux & unfix *)
C00188 00043 procedure doAffix
C00194 00044 procedure doUnfix
C00195 00045 (* aux routines for motions: forcebits, getMechbits, moveStart, moveEnd, moveRetry *)
C00210 00046 procedure doCmon
C00217 00047 procedure doMove
C00237 00048 procedure doOperate
C00241 00049 procedure doOpen (* & doClose *)
C00247 00050 procedure doCenter
C00249 00051 procedure doStop
C00251 00052 procedure doRetry
C00253 00053 procedure doSetbase
C00254 00054 procedure doWrist
C00256 00055 (* command loop *)
C00267 ENDMK
C⊗;
(*$E+ Routines to interpret an AL program *)
(*$S3000 use a large codesize *)
program interp;
(* The following bits are used during calls to the ARM servo *)
const
YARMDEV = (*1B*) 1; (* device bits for: yellow arm *)
YHANDDEV = (*2B*) 2;
BARMDEV = (*4B*) 4;
BHANDDEV = (*10B*) 8;
VISEDEV = (*20B*) 16;
DRIVERDEV = (*40B*) 32;
GARMDEV = (*100B*) 64;
GHANDDEV = (*200B*) 128;
RARMDEV = (*400B*) 256;
RHANDDEV = (*1000B*) 512;
FTABLE = (*400B*) 256; (* Force trans (C) in table coordinates *)
FHAND = 0; (* " " " " hand coordinate system *)
XFORCE = 0; (* Force along X direction of C *)
YFORCE = (*1000B*) 512; (* " " Y " " " *)
ZFORCE = (*2000B*) 1024; (* " " Z " " " *)
XMOMENT = (*3000B*)1536; (* Moment about X direction of C *)
YMOMENT = (*4000B*)2048; (* " " Y " " " *)
ZMOMENT = (*5000B*)2560; (* " " Z " " " *)
FSTOP = (*10000B*)4096; (* In addition to starting cmon, stop arm *)
SIGMAG = (*20000B*)8192; (* Test only magnitude of forces *)
SIGGE = (*100000B*) 32768; (* Start cmon if force ≥ specified value *)
SIGLT = 0; (* " " " " < " " *)
BARMPOWER = 1; (* bit defs - used in response to initarmscmd *)
YARMPOWER = 2;
GARMPOWER = 4;
RARMPOWER = (*10B*) 8;
GARMCAL = (*400B*) 256;
RARMCAL = (*1000B*) 512;
NULLINGCB = 1; (* control bits for trajectory specs *)
WOBBLECB = 2;
DURLBCB = (*20B*) 16; (* Duration: lower, upper & exact bounds *)
DURUBCB = (*40B*) 32;
DUREQCB = (*60B*) 48;
VELOCCB = (*100B*) 64;
CODECB = (*200B*) 128;
VIAPTCB = (*400B*) 256;
DEPRPTCB = (*1000B*) 512;
APPRPTCB = (*2000B*) 1024;
DESTPTCB = (*10000B*) 4096;
maxInt = 32767; (* max 16 bit integer *)
(* Control character definitions and others *)
ctlA = 01; (* Control-A *)
ctlB = 02;
ctlC = 03;
ctlD = 04;
ctlE = 05;
ctlF = 06;
ctlG = 07;
ctlH = 08;
ctlI = 09;
ctlJ = 10;
ctlK = 11;
ctlL = 12;
ctlM = 13;
ctlN = 14;
ctlO = 15;
ctlP = 16;
ctlQ = 17;
ctlR = 18;
ctlS = 19;
ctlT = 20;
ctlU = 21;
ctlV = 22;
ctlW = 23;
ctlX = 24;
ctlY = 25;
ctlZ = 26;
FF = ctlL; (* Form feed *)
CR = ctlM; (* Carriage return *)
LF = ctlJ; (* Line feed *)
TAB = ctlI; (* Tab *)
ESC = 27; (* Escape *)
smallA = 97; (* Lowercase a (sail pascal converts all input to upper case) *)
smallZ = 122;
undline = 95; (* Underline _ *)
vbar = 124; (* Vertical bar | *)
lbrace = 123; (* Left brace (curly bracket) *)
rbrace = 126; (* and right brace *)
deletekey = 127; (* Delete key code *)
type
(* random type declarations for OMSI/SAIL compatibility *)
(* ascii = char; *)
atext = packed file of ascii;
(* atext = text; *)
(* Here are all the pointer-type definitions. Since the various *)
(* records reference each other so much, we have to put them all here. *)
vectorp = ↑vector;
transp = ↑trans;
strngp = ↑strng;
eventp = ↑event;
framep = ↑frame;
statementp = ↑statement;
varidefp = ↑varidef;
nodep = ↑node;
identp = ↑ident;
tokenp = ↑token;
reswordp = ↑resword;
pdbp = ↑pdb;
envheaderp = ↑envheader;
enventryp = ↑enventry;
environp = ↑environment;
cmoncbp = ↑cmoncb;
messagep = ↑message;
(* datatype definitions *)
datatypes = (pconstype, varitype, svaltype, vectype, rottype, transtype,
frametype, eventtype, strngtype, labeltype, proctype, arraytype,
reftype, valtype, cmontype, nulltype, undeftype,
dimensiontype, mactype, macargtype, freevartype);
scalar = real;
vector = record refcnt: integer; val: array [1..3] of real end;
trans = record refcnt: integer; val: array [1..3,1..4] of real end;
cstring = packed array [0..9] of ascii;
c4str = packed array [0..3] of ascii;
c5str = packed array [0..4] of ascii;
c20str = packed array [0..19] of ascii;
strng = record
next: strngp;
ch: cstring;
end;
event = record
next: eventp; (* all events are on one big list *)
count: integer;
waitlist: pdbp;
end;
frame = record
vari: varidefp; (* back pointer to variable name & info *)
calcs: nodep; (* affixment info *)
case ftype: boolean of (* frame = true, device = false *)
true: (valid: integer; val, fdepr: transp; dcntr: integer; dev: framep);
false: (mech: integer; case sdev: boolean of
true: (sdest: real); false: (tdest,appr,depr: transp));
(* sdev = true for scalar devices, false for frames *)
end;
byte = 0..255; (* doesn't really belong here, but... *)
(* statement definitions *)
stmntypes = (progtype, blocktype, coblocktype, endtype, coendtype,
fortype, iftype, whiletype, untiltype, casetype,
calltype, returntype,
printtype, prompttype, pausetype, aborttype, assigntype,
signaltype, waittype, enabletype, disabletype, cmtype,
affixtype, unfixtype,
movetype,operatetype,opentype,closetype,centertype,floattype,
stoptype, retrytype, toarmtype,
requiretype, definetype, macrotype, commenttype, dimdeftype,
setbasetype, wristtype, saytype, declaretype, emptytype,
evaltype);
(* more??? *)
statement = packed record
next, last: statementp;
stlab: varidefp;
exprs: nodep; (* any expressions used by this statement *)
nlines: integer;
bpt,bad: boolean;
case stype: stmntypes of
progtype: (pcode: statementp; errors: integer);
blocktype,
declaretype,
endtype,
coendtype: (bcode, bparent: statementp; blkid: identp;
level, numvars: 0..255; variables: varidefp);
coblocktype: (threads: nodep; nthreads: integer; cblkid: identp);
fortype: (forvar, initial, step, final: nodep; fbody: statementp);
whiletype,
untiltype: (cond: nodep; body: statementp);
casetype: (index: nodep; range, ncases: integer; caselist: nodep);
iftype: (icond: nodep; thn, els: statementp);
pausetype: (ptime: nodep);
prompttype,
printtype,
aborttype,
saytype: (plist: nodep; debugLev: integer);
returntype: (retval, rproc: nodep);
evaltype,
calltype,
assigntype: (what, aval: nodep);
affixtype,
unfixtype: (frame1, frame2, byvar, atexp: nodep; rigid: boolean);
signaltype,
waittype: (event: nodep);
movetype,
operatetype,
opentype,
closetype,
centertype,
floattype,
stoptype,
setbasetype,
wristtype: (cf, clauses, fvec, tvec: nodep);
retrytype: (rcode, rparent: statementp; olevel: integer);
cmtype: (oncond: nodep; conclusion: statementp;
deferCm, exprCm: boolean; cdef: varidefp);
enabletype,
disabletype: (cmonlab: varidefp);
requiretype: (rfil: boolean; rfils: strngp; rfilen: integer);
definetype: (macname,mpars: varidefp; macdef: tokenp);
commenttype: (len: integer; str: strngp; cbody: statementp);
dimdeftype: (dimname: varidefp; dimexpr: nodep);
end;
(* auxiliary definitions: variable, etc. *)
varidef = packed record
next,dnext: varidefp;
name: identp;
level: 0..255; (* environment level *)
offset: 0..255; (* environment offset *)
dtype: varidefp; (* to hold the dimension info *)
tbits: 0..15; (* special type bits: array = 1, proc = 2, ref = 4 & ? *)
dbits: 0..15; (* for use by debugger/interpreter *)
case vtype: datatypes of
arraytype: (a: nodep);
proctype: (p: nodep);
labeltype,
cmontype: (s: statementp);
mactype: (mdef: statementp);
macargtype: (marg: tokenp);
pconstype: (c: nodep);
dimensiontype: (dim: nodep);
end;
(* definition of the ubiquitous NODE record *)
nodetypes = (exprnode, leafnode, listnode, clistnode, colistnode, forvalnode,
deprnode, viaptnode, apprnode, destnode, bynode, durnode,
sfacnode, wobblenode, swtnode, nullingnode, wristnode, cwnode,
arrivalnode, departingnode,
ffnode, forcenode, stiffnode, gathernode, cmonnode, errornode,
calcnode, arraydefnode, bnddefnode, bndvalnode,
waitlistnode, procdefnode, tlistnode, dimnode, commentnode,
linearnode, elbownode, shouldernode, flipnode);
exprtypes = ( svalop, (* scalar operators *)
sltop, sleop, seqop, sgeop, sgtop, sneop, (* relations *)
notop, orop, xorop, andop, eqvop, (* logical *)
saddop, ssubop, smulop, sdivop, snegop, sabsop, (* scalar ops *)
sexpop, maxop, minop, intop, idivop, modop,
sqrtop, logop, expop, timeop, (* functions *)
sinop, cosop, tanop, asinop, acosop, atan2op, (* trig *)
vdotop, vmagnop, tmagnop,
vecop, (* vector operators *)
vmakeop, unitvop, vaddop, vsubop, crossvop, vnegop,
svmulop, vsmulop, vsdivop, tvmulop, wrtop,
tposop, taxisop,
transop, (* trans operators *)
tmakeop, torientop, ttmulop, tvaddop, tvsubop, tinvrtop,
vsaxwrop, constrop, ftofop, deproachop, fmakeop, vmkfrcop,
ioop, (* i/o operators *)
queryop, inscalarop,
specop, (* special operators *)
arefop, callop, grinchop, macroop, vmop, adcop, dacop,
badop,
addop, subop, negop, mulop, divop, absop); (* for parsing *)
leaftypes = pconstype..strngtype;
reltypes = sltop..sgtop;
forcetypes = (force,absforce,torque,abstorque,angvelocity);
node = record
next: nodep;
case ntype: nodetypes of
exprnode: (op: exprtypes; arg1, arg2, arg3: nodep; elength: integer);
leafnode: (case ltype: leaftypes of
varitype: (vari: varidefp; vid: identp);
pconstype: (cname: varidefp; pcval: nodep);
svaltype: (s: scalar; wid: integer);
vectype: (v: vectorp);
transtype: (t: transp);
strngtype: (length: integer; str: strngp) ); (* also used by commentnodes *)
listnode: (lval: nodep);
clistnode: (cval: integer; stmnt: statementp; clast: nodep);
colistnode: (prev: nodep; cstmnt: statementp);
forvalnode: (fvar: enventryp; fstep: scalar);
arrivalnode:(evar: varidefp);
deprnode,
apprnode,
destnode: (loc: nodep; code: statementp);
bynode,
viaptnode: (vlist: boolean; via,duration,velocity: nodep; vcode: statementp);
durnode: (durrel: reltypes; durval: nodep);
sfacnode,
wobblenode,
swtnode: (clval: nodep);
nullingnode, (* true = nonulling *)
wristnode, (* = don't zero force wrist *)
cwnode, (* = counter_clockwise *)
elbownode, (* = elbow up *)
shouldernode, (* = right shoulder *)
flipnode, (* = don't flip wrist *)
linearnode: (notp: boolean); (* = linear motion *)
ffnode: (ff,cf: nodep; csys, pdef: boolean); (* true = world, false = hand *)
forcenode: (ftype: forcetypes; frel: reltypes; fval, fvec, fframe: nodep);
stiffnode: (fv, mv, coc: nodep);
gathernode: (gbits: integer);
cmonnode: (cmon: statementp; errhandlerp: boolean);
errornode: (eexpr: nodep);
calcnode: (rigid, frame1: boolean; other: framep; case tvarp: boolean of
false: (tval: transp); true: (tvar: enventryp) );
arraydefnode: (numdims: 1..10; bounds: nodep; combnds: boolean);
bnddefnode: (lower, upper: nodep);
bndvalnode: (lb, ub, mult: integer);
waitlistnode: (who: pdbp; when: integer);
procdefnode:(ptype: datatypes; level: 0..255;
pname, paramlist: varidefp; body: statementp);
tlistnode: (tok: tokenp);
dimnode: (time, distance, angle, dforce: integer);
end;
(* records for parser: ident, token, resword *)
ident = record
next: identp;
length: integer;
name: strngp;
curv: varidefp;
end;
tokentypes = (reswdtype, identtype, constype, comnttype, delimtype, labeldeftype,
macpartype);
constypes = svaltype..strngtype;
reswdtypes = (stmnttype, filtype, clsetype, decltype, optype);
filtypes = (abouttype,alongtype,attype,bytype,defertype,dotype,elsetype,
errmodestype,fromtype,handtype,intype,nonrigidlytype,rigidlytype,
sourcefiletype,steptype,thentype,totype,untltype,viatype,
withtype,worldtype,zeroedtype,oftype,wheretype,nowaittype);
clsetypes = (approachtype,arrivaltype,departuretype,departingtype,durationtype,
errortype,forcetype,forceframetype,forcewristtype,gathertype,
nonullingtype,nullingtype,stiffnesstype,torquetype,velocitytype,
wobbletype,cwtype,ccwtype,stopwaittimetype,angularvelocitytype);
token = record
next: tokenp;
case ttype: tokentypes of
constype: (cons: nodep);
comnttype: (len: integer; str: strngp);
delimtype: (ch: char);
reswdtype: (case rtype: reswdtypes of
stmnttype: (stmnt: stmntypes);
filtype: (filler: filtypes);
clsetype: (clause: clsetypes);
decltype: (decl: datatypes);
optype: (op: exprtypes) );
identtype: (id: identp);
labeldeftype: (lab: varidefp);
macpartype: (mpar: varidefp);
end;
resword = record
next: reswordp;
length: integer;
name: strngp;
case rtype: reswdtypes of
stmnttype: (stmnt: stmntypes);
filtype: (filler: filtypes);
clsetype: (clause: clsetypes);
decltype: (decl: datatypes);
optype: (op: exprtypes);
end;
(* process descriptor blocks & environment record definitions *)
queuetypes = (nullqueue,nowrunning,runqueue,inputqueue,eventqueue,sleepqueue,
forcewait,devicewait,joinwait,proccall);
pdb = packed record
nextpdb,next: pdbp; (* for list of all/active pdb's *)
level: 0..255; (* lexical level *)
mode: 0..255; (* expression/statement/sub-statement *)
priority: 0..255;
status: queuetypes; (* what are we doing *)
env: envheaderp;
spc: statementp; (* current statement *)
epc: nodep; (* current expression (if any) *)
sp: nodep; (* intermediate value stack *)
cm: cmoncbp; (* if we're a cmon point to our definition *)
mech: framep; (* current device being used *)
linenum: integer; (* used by editor/debugger *)
case procp: boolean of (* true if we're a procedure *)
true: (opdb: pdbp; (* pdb to restore when procedure exits *)
pdef: nodep); (* procedure definition node *)
false: (evt: eventp; (* event to signal when process goes away *)
sdef: statementp); (* first statement where process was defined *)
end;
envheader = packed record
parent: envheaderp;
env: array [0..4] of environp;
varcnt: 0..255; (* # of variables in use ??? *)
case procp: boolean of (* true if we're a procedure *)
true: (proc: nodep);
false:(block: statementp);
end;
enventry = record
case etype: datatypes of
svaltype: (s: scalar);
vectype: (v: vectorp);
transtype: (t: transp);
frametype: (f: framep);
eventtype: (evt: eventp);
strngtype: (length: integer; str: strngp);
cmontype: (c: cmoncbp);
proctype: (p: nodep; penv: envheaderp);
reftype: (r: enventryp);
arraytype: (a: envheaderp; bnds: nodep);
end;
environment = record
next: environp;
vals: array [0..9] of enventryp;
end;
cmoncb = record
running, enabled: boolean; (* cmon's status *)
cmon: statementp;
pdb: pdbp;
evt: eventp;
fbits: integer; (* bits for force sensing *)
oldcmon: cmoncbp; (* for debugger *)
end;
(* definition of AL-ARM messages *)
msgtypes = (initarmscmd,calibcmd,killarmscmd,wherecmd,
abortcmd,stopcmd,movehdrcmd,movesegcmd,
centercmd,operatecmd,movedonecmd,signalcmd,readjtcmd,drivecmd,
setccmd,forcesigcmd,forceoffcmd,biasoncmd,biasoffcmd,setstiffcmd,
zerowristcmd,wristcmd,gathercmd,getgathercmd,readadccmd,writedaccmd,
errorcmd,floatcmd);
errortypes = (noerror,noarmsol,timerr,durerr,toolong,useopr,nosuchdev,featna,
unkmess,srvdead,adcdead,nozind,exjtfc,paslim,nopower,badpot,devbusy,
baddev,timout,panicb,nocart,cbound);
message = record
cmd: msgtypes;
ok: boolean;
case integer of
1: (dev, bits, n: integer;
(* (dev, bits, n, evt: integer; (* for arm code version *)
evt: eventp;
dur: real;
case integer of
1: (v1,v2,v3: real);
2: (sfac,wobble,pos: real);
3: (val,angle,mag: real);
4: (max,min: real);
5: (error: errortypes));
2: (fv1,fv2,fv3,mv1,mv2,mv3: real); (* may never use these... *)
3: (t: array [1..6] of real);
end;
interr = record
case integer of
0: (i: integer);
1: (err,foo: errortypes);
end;
(* global variables *)
var curInt, activeInts, readQueue, allPdbs: pdbp;
sysEnv: envheaderp;
clkQueue: nodep;
allEvents: eventp;
resched, running, escapeI, singleThreadMode: boolean;
etime: integer; (* used by eval *)
curtime: integer; (* who knows where this will get updated - an ast? *)
stime: integer; (* used for clock queue on 10 *)
(* tty,ttyoutput: text; (* for terminal i/o *)
msg: messagep; (* for AL-ARM interaction *)
msgp: boolean; (* flag set if any messages pending *)
inputLine: array [1..20] of ascii;
inputp: integer; (* current offset into inputLine array above *)
inputReady: boolean;
debugLevel: integer;
(* various constant pointers *)
xhat,yhat,zhat,nilvect: vectorp;
niltrans: transp;
bpark, gpark, rpark: transp; (* arm park positions *)
(* various device & variable pointers *)
speedfactor: enventryp;
garm: framep;
(* external routines *)
procedure initAlloc; extern; (* from ALLOC.PAS *)
function newVector: vectorp; extern;
procedure relVector(v: vectorp); extern;
function newTrans: transp; extern;
procedure relTrans(t: transp); extern;
function newNode: nodep; extern;
procedure relNode(n: nodep); extern;
function newEvent: eventp; extern;
procedure relEvent(n: eventp); extern;
function newEentry: enventryp; extern;
procedure relEentry(n: enventryp); extern;
function newCmoncb: cmoncbp; extern;
procedure relCmoncb(n: cmoncbp); extern;
function newstrng: strngp; extern;
procedure relstrng(n: strngp); extern;
function newIdent: identp; extern;
procedure relIdent(n: identp); extern;
function newVaridef: varidefp; extern;
procedure relVaridef(n: varidefp); extern;
function newFrame: framep; extern;
procedure relFrame(n: framep); extern;
function newEheader: envheaderp; extern;
procedure relEheader(n: envheaderp); extern;
function newStatement: statementp; extern;
procedure relStatement(n: statementp); extern;
function newPdb: pdbp; extern;
procedure relPdb(n: pdbp); extern;
function newEnvironment: environp; extern;
procedure relEnvironment(n: environp); extern;
function sind(d: real): real; extern; (* from ARITH.PAS *)
function cosd(d: real): real; extern;
function tand(d: real): real; extern;
function asin(x: real): real; extern;
function acos(x: real): real; extern;
function atan2(x,y: real): real; extern;
function vdot (u,v: vectorp): scalar; extern;
function vmagn (v: vectorp): scalar; extern;
function vmake (a,b,c: scalar): vectorp; extern;
function svmul (s: scalar; v: vectorp): vectorp; extern;
function vsdiv (v: vectorp; s: scalar): vectorp; extern;
function vadd (u,v: vectorp): vectorp; extern;
function vsub (u,v: vectorp): vectorp; extern;
function unitv (v: vectorp): vectorp; extern;
function vcross (u,v: vectorp): vectorp; extern;
function tvmul (t: transp; v: vectorp): vectorp; extern;
function tpos (t: transp): vectorp; extern;
function torient (t: transp): transp; extern;
function taxis (t: transp): vectorp; extern;
function tmagn (t: transp): scalar; extern;
function tmake (t: transp; v: vectorp): transp; extern;
function tvadd (t: transp; v: vectorp): transp; extern;
function tvsub (t: transp; v: vectorp): transp; extern;
function ttmul (t1,t2: transp): transp; extern;
function tinvrt (t: transp): transp; extern;
function vsaxwr(ax: vectorp; w: real): transp; extern;
function construct(org,vx,vxy: vectorp): transp; extern;
function vmkfrc(v: vectorp): transp; extern;
function getsysVars: varidefp; extern; (* from PARSE.PAS *)
(* function startArm: boolean; extern; (* from RSXMSG.PAS *)
(* procedure initMsg(var buf: messagep; var flag: boolean); extern;
function SendArm: boolean; extern;
function GetArm: boolean; extern;
procedure signalArm; extern; *)
function startArm: boolean; begin startArm := true; end;
function sendArm: boolean; begin sendArm := true; end;
function getArm: boolean; begin getArm := true; end;
procedure ppLine; extern; (* from EDIT.PAS *)
procedure ppOutNow; extern;
procedure ppChar(ch: ascii); extern;
procedure pp5(ch: c5str; length: integer); extern;
procedure pp10(ch: cstring; length: integer); extern;
procedure pp10L(ch: cstring; length: integer);extern;
procedure pp20(ch: c20str; length: integer); extern;
procedure pp20L(ch: c20str; length: integer); extern;
procedure ppInt(i: integer); extern;
procedure ppReal(r: real); extern;
procedure ppStrng(length: integer; s: strngp); extern;
procedure ppDelChar; extern;
(* procedure freeStatement(s: statementp); extern; *) (* from FREE.PAS *)
function anyChar(var ch: ascii): boolean; extern; (* from DISP.FAI *)
procedure escInit(var flg: boolean); extern;
function getCurInt: pdbp; (* SAIL - for use by EDIT *)
begin
getCurInt := curInt;
end;
procedure setCurInt(p: pdbp);
begin
curInt := p;
end;
function getAllPdbs: pdbp;
begin
getAllPdbs := allPdbs;
end;
procedure setSingleThreadMode(b: boolean);
begin
singleThreadMode := b;
end;
(* message passing routines: sendCmd, sendTrans, getReply, whereArm *)
procedure sendCmd;
var b: boolean;
begin
b := sendArm; (* send message to ARM *)
(* with msg↑ do
if not ((cmd = movesegcmd) or (cmd = movehdrcmd) or
(cmd = setccmd) or (cmd = setstiffcmd)) then signalArm; (* tell arm *)
end;
procedure sendTrans(tr: transp);
var i,j,k: integer; b: boolean;
begin
b := sendArm; (* first send over message header *)
with msg↑,tr↑ do
begin
for k := 0 to 1 do
begin
for i := 1 to 3 do
for j := 1 to 2 do t[i + 3*(j-1)] := val[i,j + 2*k];
b := sendArm; (* send half over *)
end;
if refcnt <= 0 then relTrans(tr);
end;
end;
procedure msgDispatch; forward; (* handles signals & movedone from ARM *)
procedure getReply;
var ocmd: msgtypes; b: boolean;
begin
with msg↑ do
begin
ocmd := cmd; (* remember what we're waiting for *)
sendCmd; (* send request to ARM servo *)
repeat
b := getArm; (* try to read a message packet from ARM *)
if b and (cmd <> ocmd) then (* if we got one, was it our reply? *)
begin
msgDispatch; (* deal with whatever the ARM servo sent over *)
b := false; (* keep waiting for our reply *)
end
until b; (* wait for reply *)
end;
end;
function getEntry (level, offset: byte): enventryp; forward;
function whereArm (mech: integer): transp; (* to read in the arm's position *)
var tp: transp; i,j: integer; b: boolean;
ev: enventryp; (* for sim ver *)
begin
tp := newTrans;
with msg↑,tp↑ do
begin
cmd := wherecmd;
dev := mech;
getReply; (* go get 1st message packet *)
if ok then (* check there's no error *)
begin
for i := 1 to 3 do
for j := 1 to 2 do val[i,j] := t[i + 3*(j-1)]; (* copy result *)
repeat b := getArm until b; (* get 2nd packet (guaranteed to be next) *)
for i := 1 to 3 do
for j := 3 to 4 do val[i,j] := t[i + 3*(j-3)]; (* copy result *)
(* for simulation version *)
relTrans(tp);
case mech of (* get device offset *)
BARMDEV: i := 0;
GARMDEV: i := 4;
RARMDEV: i := 8;
end;
ev := getEntry(0,i);
tp := ev↑.f↑.tdest; (* use wherever last move was to *)
end
else
begin (* *** ERROR - maybe we should complain??? *** *)
relTrans(tp); (* don't need this anymore *)
tp := niltrans;
end;
end;
whereArm := tp;
end;
(* aux routines: push, pop, upTrans, getELev, getEntry, getVar, gtVarn, getVal, setVal, getNval *)
procedure push (n: nodep);
begin (* no need to check for overflow *)
n↑.next := curInt↑.sp;
curInt↑.sp := n;
end;
function pop: nodep;
begin
pop := curInt↑.sp;
if curInt↑.sp = nil then
begin (* **** error - stack underflow **** *)
pp20L('Value Stack Underflo',20); ppChar('w'); ppLine;
(* code to show where error occurred & to maybe recover??? *)
end
else curInt↑.sp := curInt↑.sp↑.next;
end;
procedure upTrans (var t: transp; tp: transp);
begin
if tp <> nil then tp↑.refcnt := tp↑.refcnt + 1; (* indicate new trans is in use *)
if t <> nil then (* check for old value *)
begin
t↑.refcnt := t↑.refcnt - 1; (* we're done with trans now *)
if t↑.refcnt <= 0 then relTrans(t); (* release it if no one else wants it *)
end;
t := tp; (* copy new trans pointer *)
end;
function envlookup (offset: integer; envhdr: envheaderp): enventryp;
var i,j,k: integer; env: environp;
begin
i := offset div 10; (* which environment block *)
j := offset mod 10; (* entry in environment block *)
if i < 5 then env := envhdr↑.env[i] (* use direct look-up *)
else begin (* run through linked list *)
env := envhdr↑.env[4];
for k := 5 to i do env := env↑.next;
end;
envlookup := env↑.vals[j];
end;
function getELev(hdr: envheaderp): integer;
begin
if hdr = sysEnv then getELev := 0
else if hdr↑.procp then getELev := hdr↑.proc↑.level
else getELev := hdr↑.block↑.level;
end;
function getEntry (* (level, offset: byte): enventryp; *);
var hdr: envheaderp;
begin
if level = 0 then hdr := sysEnv (* level zero is predefined system variables *)
else
begin
hdr := curInt↑.env; (* look up the env entry given level-offset *)
while level < getELev(hdr) do hdr := hdr↑.parent; (* move up a level *)
if level <> getELev(hdr) then (* yow!!! no environment exists!!! *)
begin
pp20L('Attempt to access no',20); pp20('n-existent environme',20);
pp20('nt - good luck! ',16); ppLine;
end;
end;
getEntry := envlookup(offset,hdr);
end;
function getVar (level, offset: byte): enventryp;
var entry: enventryp; i, j: integer; p, b: nodep;
begin
entry := getEntry(level,offset); (* get the environment entry *)
while entry↑.etype = reftype do entry := entry↑.r; (* resolve indirect refs *)
if entry↑.etype = arraytype then (* do array reference *)
begin
b := entry↑.bnds;
j := 0;
repeat
p := pop; (* get this subscript's value *)
i := round(p↑.s);
relNode(p);
if i < b↑.lb then (* subscript error *)
begin
pp20L('Subscript index less',20); pp20(' than lower bound: ',19);
ppInt(i); ppLine;
i := b↑.lb
end
else if i > b↑.ub then (* subscript error *)
begin
pp20L('Subscript index grea',20); pp20('ter than lower bound',20);
pp5(': ',2); ppInt(i); ppLine;
i := b↑.ub
end;
j := j + b↑.mult * (i - b↑.lb);
b := b↑.next;
until b = nil;
entry := envlookup(j,entry↑.a); (* lookup the array entry *)
end;
getVar := entry;
end;
function gtVarn (n: nodep): enventryp;
begin
with n↑ do
if ntype = leafnode then
with vari↑ do gtVarn := getVar(level,offset) (* access simple var *)
else
with arg1↑.vari↑ do gtVarn := getVar(level,offset); (* access array var *)
end;
procedure getFrame (f: framep; r: nodep); forward;
procedure getVal (level, offset: byte);
var entry: enventryp; res: nodep;
begin
entry := getVar(level,offset); (* look up environment entry for variable *)
res := newNode;
res↑.ntype := leafnode;
res↑.ltype := entry↑.etype; (* copy datatype of result *)
if entry↑.etype = svaltype then res↑.s := entry↑.s (* it's a scalar *)
else if entry↑.etype <> frametype then (* it's a vector, trans or string *)
with res↑ do
begin
v := entry↑.v; (* copy pointer *)
str := entry↑.str;
if v = nil then
if ltype = vectype then v := nilvect
else if ltype = transtype then t := niltrans
else length := 0;
(* complain??? *)
end
else
begin
res↑.ltype := transtype;
getFrame(entry↑.f,res);
end;
push(res); (* store the value on the stack *)
end;
procedure change (f: framep; res: nodep); forward;
procedure setVal (level, offset: byte);
var entry: enventryp; res: nodep;
begin
entry := getVar(level,offset); (* look up environment entry for variable *)
res := pop; (* pop value off of stack *)
with entry↑ do
if etype = svaltype then s := res↑.s (* it's a scalar *)
else if etype = vectype then
begin
with res↑.v↑ do refcnt := refcnt + 1; (* indicate new vector is in use *)
if v <> nil then
begin
v↑.refcnt := v↑.refcnt - 1; (* we're done with vector now *)
if v↑.refcnt <= 0 then relVector(v); (* release it if no one wants it *)
end;
v := res↑.v; (* copy new vector pointer *)
end
else if etype = transtype then upTrans(t,res↑.t) (* update trans with new value *)
else if etype = strngtype then
begin
length := res↑.length;
str := res↑.str; (* copy new string pointer *)
end
else change(f,res); (* change frame's value, updating affixed frames *)
relNode(res); (* free node up *)
end;
function getNval(n: nodep; var b: boolean): nodep;
begin
b := false;
with n↑ do
if (ntype <> leafnode) or (ltype = varitype) then
begin n := pop; b := true end;
if n <> nil then
if n↑.ltype = pconstype then
begin n := n↑.pcval; b := false end;
getNval := n;
end;
(* aux routines: getPdb, freePdb, getEvent, freeEvent *)
function getPdb: pdbp;
var p: pdbp;
begin
p := newPdb;
with p↑ do
begin (* initialize it somewhat *)
nextPdb := allPdbs;
allPdbs := p; (* add us to list of all processes *)
next := nil;
if curInt <> nil then
begin
env := curInt↑.env;
level := getELev(env) + 1;
priority := curInt↑.priority;
cm := curInt↑.cm;
end
else
begin
env := sysEnv;
level := 1;
priority := 0;
cm := nil;
end;
status := nullqueue;
mode := 0;
spc := nil;
epc := nil;
sp := nil;
mech := nil;
procp := false;
evt := nil;
end;
getPdb := p;
end;
procedure freePdb(p: pdbp);
var po: pdbp; b: boolean;
begin (* remove pdb from list *)
if allPdbs = p then allPdbs := p↑.nextPdb
else
begin
po := allPdbs;
b := false;
repeat (* find pdb in list *)
if po↑.nextPdb = p then b := true else po := po↑.nextPdb
until b or (po = nil);
if b then po↑.nextPdb := p↑.nextPdb; (* splice us out of list *)
(* *** else complain??? *** *)
end;
relPdb(p);
end;
function getEvent: eventp;
var e: eventp;
begin
e := newEvent;
e↑.next := allEvents; (* add to list of all events *)
allEvents := e;
e↑.count := 0;
e↑.waitlist := nil;
getEvent := e;
end;
procedure freeEvent(e: eventp);
var eo: eventp; b: boolean;
begin (* remove event from list *)
if allEvents = e then begin allEvents := e↑.next; b := true end
else
begin
eo := allEvents;
b := false;
repeat (* find event in list *)
if eo↑.next = e then b := true else eo := eo↑.next
until b or (eo = nil);
if b then eo↑.next := e↑.next; (* splice us out of list *)
end;
if b then relEvent(e); (* if not in list already released *)
end;
(* graph structure routines: nextTime, getFrame, getDevice, feval, eval, change, invalidate, stvals ... *)
procedure nextTime;
begin
if etime = Maxint then etime := 1 (* should reset all invalid frames, but ... *)
else etime := etime + 1;
end;
procedure eval (f: framep);
var calc: nodep; b: boolean; f2, tr: transp;
begin
if f↑.valid <> etime then (* Haven't looked at it yet *)
begin
f↑.valid := etime; (* Mark it *)
calc := f↑.calcs; (* Get list of calculators *)
b := true;
while (calc <> nil) and b do (* See if someone it's affixed to is now valid *)
if (calc↑.ntype = calcnode) and (calc↑.rigid or calc↑.frame1) then
with calc↑.other↑ do (* A possibility, look at other frame *)
begin
if not ftype then (* See if it's a device or frame *)
begin (* It's a device - use it to compute current value *)
f2 := whereArm(mech); (* Get current device pos *)
b := false; (* No need to look further *)
end
else if (dcntr=0) and (valid=0) then (* not dynamic & valid frame *)
begin f2 := val; b := false end
else calc := calc↑.next (* dynamic or not valid - try next *)
end
else calc := calc↑.next; (* not a calc, or nonrigid and frame2 - try next *)
if calc = nil then
begin (* Check calcs again - this time trying to evaluate other frame *)
calc := f↑.calcs;
b := true;
while (calc <> nil) and b do
if (calc↑.ntype = calcnode) and (calc↑.rigid or calc↑.frame1) then
begin
eval(calc↑.other); (* Try to get a value for it *)
if calc↑.other↑.valid=0 then (* Is it now valid? *)
begin f2 := calc↑.other↑.val; b := false end (* Yes - all done *)
else calc := calc↑.next (* still not valid - try next *)
end
else calc := calc↑.next; (* not a calc, or nonrigid and frame2 - try next *)
end;
if calc <> nil then
with calc↑ do
begin (* use other frame to evaluate desired one & return success *)
if tvarp then tr := tvar↑.t else tr := tval; (* explicitly named trans? *)
if not frame1 then tr := tinvrt(tr); (* second := inv(trans) * first *)
upTrans(f↑.val,ttmul(tr,f2)); (* first := trans * second *)
f↑.valid := 0; (* Mark it as now valid *)
end;
end;
end;
function feval (f: framep): transp;
begin
if not f↑.ftype then
begin (* If device use its current value *)
feval := whereArm(f↑.mech); (* Get current device pos *)
end
else (* frame *)
begin
if (f↑.dcntr<>0) or (f↑.valid<>0) then (* dynamic frame or not valid? *)
begin (* Need to calculate current value *)
nextTime; (* update eval time *)
eval(f); (* try to evaluate the variable *)
end;
if f↑.valid = 0 then feval := f↑.val (* copy trans pointer *)
else feval := niltrans; (* but always return something *)
end;
end;
function invalidate (f: framep): boolean;
var calc: nodep; b: boolean;
begin
(* invalidate frame & all other frames affixed either rigidly or
non-rigidly with this being frame2,
else indicate we need to modify non-rigid trans. *)
b := false; (* assume no updating of non-rigid relationships *)
if etime <> f↑.valid then (* haven't marked this one yet *)
with f↑ do
begin
if valid = 0 then upTrans(val,nil); (* flush old value *)
valid := etime; (* mark us as having an invalid value *)
calc := calcs;
while calc <> nil do (* invalidate everyone we're affixed to *)
begin (* rigidly or if we're frame 2 *)
if (calc↑.ntype = calcnode) and (calc↑.rigid or (not calc↑.frame1))
then b := b or invalidate(calc↑.other) (* go invalidate frame *)
else b := true; (* found a non-rigid affixment to update *)
calc := calc↑.next; (* now repeat with next calc *)
end;
end;
invalidate := b;
end;
procedure stvals (f: framep);
var calc,c2: nodep; t,val: transp; f2: framep;
begin
calc := f↑.calcs;
val := f↑.val; (* frames current value *)
while calc <> nil do (* update everyone we're affixed to *)
with calc↑ do
begin
f2 := other;
if (ntype = calcnode) and (rigid or (not frame1)) then
begin (* see if we need to update this frame *)
if f2↑.valid <> 0 then (* haven't updated it yet *)
begin
if tvarp then t := tvar↑.t else t := tval; (* explicitly named trans? *)
if frame1 then t := tinvrt(t); (* second := inv(trans) * first *)
upTrans(f2↑.val,ttmul(t,val)); (* first := trans * second *)
f2↑.valid := 0; (* Mark it as now valid *)
stvals(f2); (* and go update its affixments *)
end
end
else
begin (* need to update relation trans *)
t := feval(f2); (* get a value for f2 *)
t := ttmul(val,tinvrt(t)); (* compute new relation trans *)
if tvarp then upTrans(tvar↑.t,t)
else
begin
upTrans(tval,t); (* store it *)
c2 := f2↑.calcs; (* now go fix trans up in f2's calc list *)
while c2↑.other <> f do c2 := c2↑.next; (* find other calc of pair *)
upTrans(c2↑.tval,t); (* copy trans to it too *)
end;
end;
calc := calc↑.next; (* move on to next one *)
end;
end;
procedure change (* f: framep; res: nodep *);
var calc: nodep; b: boolean;
begin
if f↑.dcntr=0 then (* if not dynamic *)
begin
nextTime;
b := invalidate(f); (* b = true if any non-rigid affixments need updating *)
f↑.val := res↑.t; (* copy trans pointer *)
f↑.val↑.refcnt:=f↑.val↑.refcnt + 1; (* mark trans in use *)
f↑.valid := 0; (* mark us as having a valid value *)
if b then stvals(f); (* go fix up the non-rigid relationships *)
end
else begin
pp20L('Can''t assign to dyna',20); pp10('mic frames',10); ppLine;
(* maybe also give name of frame?? *)
end;
end;
procedure getDevice (f: framep; r: nodep);
var i: integer; ev: enventryp; (* for sim ver *)
begin
if f↑.sdev then
with msg↑ do
begin
cmd := wherecmd;
dev := f↑.mech;
getReply; (* have ARM servo read in the hand/device value *)
r↑.s := val;
r↑.ltype := svaltype;
(* for simulation version *)
case dev of (* get device offset *)
BHANDDEV: i := 2;
GHANDDEV: i := 6;
RHANDDEV: i := 10;
DRIVERDEV: i := 12;
VISEDEV: i := 16;
end;
ev := getEntry(0,i);
r↑.s := ev↑.f↑.sdest; (* use where ever last move was to *)
end
else
r↑.t := whereArm(f↑.mech); (* go read in the arm's position *)
end;
procedure getFrame (* f: framep; r: nodep *);
begin
if not f↑.ftype then getDevice(f,r) (* If device get its current value *)
else (* frame *)
begin
if (f↑.dcntr<>0) or (f↑.valid<>0) then (* dynamic frame or not valid? *)
begin (* Need to calculate current value *)
nextTime; (* update eval time *)
eval(f); (* try to evaluate the variable *)
end;
r↑.t := f↑.val; (* copy trans pointer *)
if r↑.t = nil then r↑.t := niltrans; (* always return something *)
(* complain though??? *)
end;
end;
(* aux routines to create & destroy variables: enterEntry,makeCmon,makeVar,killVar,killEnv,killNode,killStack *)
function enterEntry (var i,j: integer; var env: environp;
envhdr: envheaderp; v: varidefp): enventryp;
var e: enventryp; k: integer;
begin
if j = 9 then (* need to allocate new environment record *)
begin
env↑.next := newEnvironment;
env := env↑.next;
env↑.next := nil;
for k := 0 to 9 do env↑.vals[k] := nil;
j := 0;
i := i + 1;
if i < 5 then envhdr↑.env[i] := env;
end
else j := j + 1;
k := 10 * i + j;
if k > envhdr↑.varcnt then envhdr↑.varcnt := k;
e := newEentry; (* get an environment entry for the variable *)
env↑.vals[j] := e;
e↑.etype := v↑.vtype; (* copy datatype of variable *)
if e↑.etype = rottype then e↑.etype := transtype; (* rots are transes internally *)
enterEntry := e;
end;
procedure makeCmon(e: enventryp; vari: varidefp);
var c: cmoncbp;
begin
c := newCmoncb;
with c↑ do
begin
cmon := vari↑.s; (* point to cmon definition *)
enabled := false;
running := false;
pdb := getPdb; (* get us a pdb for later *)
oldcmon := e↑.c; (* remember if we're pushing anyone *)
if c↑.cmon↑.oncond↑.ntype = forcenode then
evt := getEvent (* we'll need an event later *)
else evt := nil;
end;
with c↑.pdb↑ do
begin (* set up pdb *)
priority := (priority mod 10) + 1; (* base level priority *)
spc := c↑.cmon;
sdef := spc;
cm := c; (* point to cmon def *)
opdb := curInt; (* pointer to parent pdb so we can get mech bits *)
end;
e↑.c := c;
end;
procedure makeVar(e: enventryp; vari: varidefp; tbits: integer);
var i,j,k,size: integer; envhdr: envheaderp; env: environp; ep: enventryp;
b,bo,bd: nodep;
function getBound (n: nodep): integer;
var e: enventryp;
begin
if n↑.ntype = exprnode then (* value on stack *)
begin n := pop; getBound := round(n↑.s) end
else if n↑.ltype = svaltype then getBound := round(n↑.s) (* constant val *)
else if n↑.ltype = pconstype then
getBound := round(n↑.pcval↑.s) (* predeclared constant *)
else
begin (* variable value *)
with n↑.vari↑ do e := getVar(level,offset);
getBound := round(e↑.s);
end;
end;
function getSize (b: nodep): integer;
begin
if b↑.next = nil then b↑.mult := 1
else b↑.mult := getSize(b↑.next);
getSize := b↑.mult * (b↑.ub - b↑.lb + 1);
end;
begin
with e↑ do
begin
if tbits = 1 then etype := arraytype
else if tbits = 2 then etype := proctype
else if tbits >= 4 then etype := reftype;
case etype of
svaltype: s := 0.0;
vectype,
transtype: v := nil;
frametype: begin
f := newFrame;
f↑.vari := vari;
f↑.calcs := nil;
f↑.ftype := true;
f↑.valid := -1;
f↑.val := nil;
f↑.fdepr := nil;
f↑.dcntr := 0;
f↑.dev := nil;
end;
eventtype: evt := getEvent;
strngtype: begin length := 0; str := nil end;
cmontype: begin
c := nil;
makeCmon(e,vari);
end;
proctype: begin
etype := proctype; (* fix up type field *)
p := vari↑.p;
penv := curInt↑.env;
end;
arraytype: begin
bd := vari↑.a↑.bounds;
bo := nil;
while bd <> nil do (* bind the array bounds *)
begin
b := newNode;
if bo = nil then e↑.bnds := b else bo↑.next := b;
bo := b;
b↑.ntype := bndvalnode;
b↑.lb := getBound(bd↑.lower);
b↑.ub := getBound(bd↑.upper);
bd := bd↑.next
end;
size := getSize(e↑.bnds);
envhdr := newEheader;
envhdr↑.varcnt := 0;
e↑.a := envhdr;
env := newEnvironment;
env↑.next := nil;
envhdr↑.env[0] := env;
for j := 1 to 4 do envhdr↑.env[j] := nil;
for j := 0 to 9 do env↑.vals[j] := nil;
i := 0;
j := -1;
for k := 1 to size do
begin
ep := enterEntry(i,j,env,envhdr,vari);
makeVar(ep,vari,0); (* make variable environment entry *)
end;
for i := j+1 to 9 do env↑.vals[i] := nil;
end;
end;
end;
end;
procedure unfix(f1,f2: framep); forward;
procedure killVar(e: enventryp);
var j,k,size: integer; envhdr: envheaderp; env,eo: environp; ep: enventryp;
b,bo: nodep; pp: pdbp; cp: cmoncbp;
begin
with e↑ do
case etype of
svaltype,
strngtype: begin end; (* nothing to do *)
vectype: if v <> nil then (* check for old value *)
begin
v↑.refcnt := v↑.refcnt - 1; (* we're done with vector now *)
if v↑.refcnt <= 0 then relVector(v); (* release it if no one else wants it *)
end;
transtype: upTrans(t,nil);
frametype: begin
while f↑.calcs <> nil do
unfix(f,f↑.calcs↑.other); (* unfix us from everyone *)
upTrans(f↑.val,nil); (* flush our current value *)
relFrame(f); (* flush frame *)
end;
eventtype: begin
(* *** what to do with those processes waiting on this event? *** *)
pp := evt↑.waitlist;
while pp <> nil do
begin pp↑.status := nullqueue; pp := pp↑.next end;
freeEvent(evt);
end;
cmontype: repeat
if c↑.cmon↑.oncond↑.ntype = forcenode then freeEvent(c↑.evt);
freePdb(c↑.pdb); (* now it's ok to flush its pdb *)
cp := c↑.oldcmon; (* did we have several copies active? *)
relCmoncb(c); (* and also free up its cmoncb *)
c := cp;
until cp = nil;
arraytype: begin
b := e↑.bnds;
size := b↑.mult * (b↑.ub - b↑.lb + 1); (* get array size *)
while b <> nil do begin bo := b; b := b↑.next; relNode(bo) end;
envhdr := e↑.a;
env := envhdr↑.env[0];
relEheader(envhdr);
j := -1;
for k := 1 to size do
begin
if j = 9 then
begin eo := env; env := env↑.next; relEnvironment(eo); j := 0 end
else j := j + 1;
ep := env↑.vals[j];
killVar(ep); (* kill variable environment entry *)
end;
relEnvironment(env);
end;
(* nothing to do for procedures or indirect references *)
end;
relEentry(e);
e := nil;
end;
procedure killEnv;
var envhdr: envheaderp; envir,eo: environp; e: enventryp; j: integer;
begin
if (curInt↑.env <> sysEnv) and (curInt↑.env↑.varcnt < 255) then
begin (* varcnt check is so flushall doesn't have us kill it twice *)
with curInt↑ do
begin
envhdr := env;
env := envhdr↑.parent;
end;
envhdr↑.varcnt := 255;
envir := envhdr↑.env[0];
relEheader(envhdr);
j := 0;
while envir <> nil do (* deallocate variables *)
begin
e := envir↑.vals[j];
if e <> nil then killVar(e); (* kill var's environment entry *)
if j = 9 then
begin
eo := envir;
envir := envir↑.next;
relEnvironment(eo);
j := 0
end
else j := j + 1;
end;
end
else curInt↑.env := sysEnv;
end;
procedure killNode(n: nodep);
begin
with n↑ do
if ntype = leafnode then
case ltype of
vectype: if v↑.refcnt <= 0 then relVector(v);
transtype: if t↑.refcnt <= 0 then relTrans(t);
others: begin end; (* nothing to do *)
end;
relNode(n);
end;
procedure killStack;
var n,np: nodep;
begin
n := curInt↑.sp; (* top of stack *)
while n <> nil do
begin
np := n↑.next;
killNode(n);
n := np;
end;
end;
(* aux io routines: prntSval, prntVec, prntTrans, prntStrng, prntPlist, onum, prntVar, badjoints *)
procedure prntSval(s: real);
var si: real;
begin
if s < maxInt then
begin
si := trunc(s);
s := si + round(1000*(s-si))/1000;
end;
ppReal(s);
end;
procedure prntVec(v: vectorp);
var i: integer;
begin
pp10('vector( ',7);
with v↑ do
for i := 1 to 3 do
begin
prntSval(val[i]);
if i = 3 then ppChar(')') else ppChar(',');
end;
ppOutNow;
end;
procedure prntTrans(t: transp);
var i: integer; v: vectorp;
begin
with t↑ do
begin
refcnt := refcnt + 1;
pp10('trans(rot(',10);
v := taxis(t); prntVec(v); relVector(v);
ppChar(',');
prntSval(tmagn(t));
pp10('),vector( ',9);
for i := 1 to 3 do
begin prntSval(val[i,4]); if i = 3 then ppChar(')') else ppChar(',') end;
ppChar(')');
refcnt := refcnt - 1;
end;
ppLine;
end;
procedure prntStrng(length: integer; s: strngp);
begin
ppStrng(length,s);
ppOutNow;
end;
procedure prntPlist(n: nodep);
var np: nodep; b: boolean;
begin
while n <> nil do (* print out the list *)
begin
np := getNval(n↑.lval,b);
if np <> nil then
begin
with np↑ do
case ltype of
svaltype: begin prntSval(s); ppOutNow end;
vectype: prntVec(v);
transtype: prntTrans(t);
strngtype: prntStrng(length,str);
end;
if b then killNode(np); (* flush used stack entry *)
end;
n := n↑.next;
end;
end;
procedure onum(s: integer);
procedure onum1(s: integer);
var i,j: integer;
begin
i := s div 8;
j := s mod 8;
if i > 0 then onum(i);
ppInt(j);
end;
begin
if s < 0 then begin ppChar('-'); s := -s end;
onum1(s);
ppOutNow;
end;
procedure prntVar(v: nodep);
var i: integer; n,p: nodep;
begin
if v = nil then pp10('Noname ',6)
else if v↑.ntype = leafnode then
with v↑.vid↑ do ppStrng(length,name) (* print variable name *)
else
begin (* array ref *)
with v↑.arg1↑.vid↑ do ppStrng(length,name); (* print variable name *)
n := v↑.arg2;
ppChar('[');
while n <> nil do
begin
p := pop; (* get this subscript's value *)
i := round(p↑.s);
ppInt(i);
relNode(p);
n := n↑.next;
if n = nil then ppChar(']') else ppChar(',');
end;
end;
ppLine;
end;
procedure badJoints(angle: integer);
var i: integer;
begin
if angle <> 0 then
begin (* tell associated joint numbers *)
pp20(' joint(s) = ',14);
i := 1;
while angle <> 0 do (* decode them *)
begin
if odd(angle) then
begin
ppInt(i);
if angle > 1 then ppChar(',');
end;
angle := angle div 2;
i := i + 1;
end;
ppLine;
end;
end;
(* aux routines: addPdb, sleep, deClkQueue, msgDispatch, swap *)
procedure addPdb(var plist: pdbp; pn: pdbp);
var p,pp: pdbp; b: boolean;
begin
if plist = nil then
begin (* empty queue - we're it *)
plist := pn;
pn↑.next := nil;
end
else if plist↑.priority < pn↑.priority then
begin (* add us to start of queue *)
pn↑.next := plist;
plist := pn;
end
else
begin (* merge us into the queue *)
p := plist;
b := true;
while (p↑.next <> nil) and b do
if p↑.next↑.priority >= pn↑.priority then p := p↑.next else b := false;
pn↑.next := p↑.next;
p↑.next := pn;
end;
end;
procedure sleep(whenV: integer);
var w,n,np: nodep; p,pp: pdbp; b: boolean; ti: integer;
begin
curInt↑.next := nil;
np := clkQueue;
n := nil;
b := true;
ti := stime;
while np <> nil do
if ti = whenV then (* add us to this wait node *)
begin
addPdb(np↑.who,curInt);
np := nil;
b := false;
end
else if ti < whenV then
begin (* move down list *)
whenV := whenV - ti; (* update relative wait time *)
n := np;
np := np↑.next;
if np <> nil then ti := np↑.when;
end
else np := nil;
if b then (* need to make a new entry *)
begin
w := newNode;
with w↑ do
begin
ntype := waitlistnode;
who := curInt;
when := whenV;
next := nil;
end;
(* request a Marktime ast to have us made active *)
if n = nil then
begin
w↑.next := clkQueue;
clkQueue := w; (* first entry in queue *)
stime := whenv; (* hack for 10 *)
end
else
begin (* add us to the queue *)
w↑.next := n↑.next;
n↑.next := w;
end;
if w↑.next <> nil then w↑.next↑.when := w↑.next↑.when - whenV;
end;
curInt↑.status := sleepqueue;
curInt := nil; (* swap in someone else *)
resched := true;
end;
procedure deClkQueue(po: pdbp);
var n,np: nodep; p,pp: pdbp; b: boolean;
begin (* remove pdb from clock queue *)
n := clkQueue;
np := nil;
b := true;
while (n <> nil) and b do
begin
p := n↑.who;
pp := nil;
while (p <> nil) and (p <> po) do begin pp := p; p := p↑.next end;
if p <> nil then (* found us, now splice us out of the list *)
begin
b := false;
if pp = nil then
begin (* we were first entry in list *)
n↑.who := p↑.next;
if n↑.who = nil then (* check if we were only entry *)
begin (* yup - remove this wait list node *)
if np <> nil then np↑.next := n↑.next (* splice out node *)
else
begin (* we were first node *)
clkQueue := n↑.next;
if n↑.next = nil then stime := 0 (* clock queue empty now *)
else stime := stime + n↑.next↑.when; (* reset new wait time *)
end;
if n↑.next <> nil then n↑.next↑.when := n↑.when + n↑.next↑.when;
relNode(n); (* done with waitlist node now *)
end
end
else pp↑.next := p↑.next; (* splice us out of list *)
end
else begin np := n; n := n↑.next end; (* try next node *)
end;
end;
procedure msgDispatch; (* handles signals & movedone from ARM *)
var p: pdbp; nd: nodep;
begin
with msg↑ do
if cmd = errorcmd then
begin
if ok then pp20L('Fatal error: ',13)
else pp10L('Warning: ',9);
case dev of (* tell which device *)
barmdev: pp10('barm - ',7);
bhanddev: pp10('bhand - ',8);
visedev: pp10('vise - ',7);
driverdev: pp10('driver - ',9);
garmdev: pp10('garm - ',7);
ghanddev: pp10('ghand - ',8);
rarmdev: pp10('rarm - ',7);
rhanddev: pp10('rhand - ',8);
others: pp20('unknown device - ',18);
end;
case error of
noarmsol: begin pp20('No arm solution foun',20); pp20('d, will use approxim',20);
pp20('ate solution. ',13) end;
nocart: begin pp20('No Cartesian path ex',20); pp20('ists between these p',20);
pp20('ath points. ',11) end;
timerr: begin pp20('Specified motion tim',20); pp20('e exceeds capabiliti',20);
pp5('es. ',3) end;
durerr: begin pp20('Motion overly constr',20); pp20('ained, will ignore g',20);
pp20('lobal time constrain',20); pp5('t. ',2) end;
toolong: begin pp20('Maximum segment time',20); pp20(' allowed is 32.2 sec',20);
pp5('onds.',5) end;
featna: begin pp20('Feature not availabl',20); pp10('e yet. ',6) end;
others: pp20('Unknown error! ',14);
end;
ppLine;
badJoints(bits); (* tell which joint(s) were bad, if any *)
end
else
begin
evt↑.count := evt↑.count + 1;
p := evt↑.waitlist; (* get pdb of process to schedule (if any) *)
if p <> nil then
begin
evt↑.waitlist := p↑.next; (* remove node from waitlist *)
p↑.status := runqueue;
addPdb(activeInts,p); (* add it to active process list *)
if curInt = nil then resched := true
else
if p↑.priority > curInt↑.priority then
resched := true; (* swap it in and swap us out *)
if cmd = movedonecmd then
begin (* need to put error bits on stack *)
nd := newNode;
with nd↑ do
begin
ntype := leafnode;
ltype := svaltype;
if ok then s := 0 else s := 128 * ord(error) + bits;
next := p↑.sp; (* push it *)
p↑.sp := nd;
end;
freeEvent(evt); (* also need to reclaim event *)
end
else if cmd <> signalcmd then
begin pp20('Unknown message of t',20); pp5('ype: ',5);
ppInt(ord(cmd)); ppLine end;
end;
end;
end;
procedure swap(newp: pdbp);
var p,po: pdbp; b: boolean; e: eventp;
begin
if newp = nil then
begin (* swap in some active process *)
curInt := activeInts;
if activeInts <> nil then activeInts := activeInts↑.next;
end
else
begin
if newp↑.status = runqueue then
begin (* remove us from activeInts list *)
if activeInts = newp then activeInts := newp↑.next;
p := activeInts;
while p↑.next <> nil do
if p↑.next = newp then p↑.next := newp↑.next (* remove us *)
else p := p↑.next;
end
else if newp↑.status = sleepqueue then deClkQueue(newp)
else if newp↑.status = eventqueue then
begin (* run through all events & remove us from event queue *)
e := allEvents;
b := true;
while b and (e <> nil) do
with e↑ do
begin
if waitlist = newp then
begin waitlist := newp↑.next; b := false end
else
begin
p := waitlist;
while b and (p <> nil) do
if p↑.next = newp then
begin p↑.next := newp↑.next; b := false end
else p := p↑.next;
end;
if b then e := next else count := count + 1;
end;
end;
if (newp <> curInt) and (curInt <> nil) then
begin
curInt↑.status := runqueue;
addPdb(activeInts,curInt); (* swap current process out *)
end;
curInt := newp; (* make new guy active *)
newp↑.next := nil;
end;
if curInt <> nil then
begin curInt↑.status := nowrunning; curInt↑.next := nil end;
end;
(* aux routines: calibrate,initArms,initWorld,consDef,passConstants,flushLevel,flushAll,unwind,flushPdb,flushKids *)
function getPromptChar: ascii;
var ch: ascii;
begin
repeat ch := getChar until ord(ch) <> lf; (* Read one character *)
if ord(ch) = cr then ch := ' '; (* Convert CR to space *)
ppChar(ch); ppOutNow; (* and echo it *)
if (smallA <= ord(ch)) and (ord(ch) <= smallZ) then
ch := chr(ord(ch)-ord(' ')); (* To upper case *)
getPromptChar := ch;
end;
procedure calibrate;
var b: boolean; i: integer; ch: ascii;
function bitOn(i: integer): boolean;
begin bitOn := true end; (* *** simulation version *** *)
(* begin bitOn := (msg↑.bits AND i) <> 0 end; (* *** non-standard Pascal *** *)
procedure whichArm;
begin
case i of (* tell which arm *)
1: pp5('GARM ',4);
2: pp5('RARM ',4);
end;
end;
begin (* hand-shaking code to calibrate arms *)
begin
for i := 1 to 2 do (* try to init just the PUMAs for now *)
begin
repeat
with msg↑ do
begin
ch := ' ';
cmd := initarmscmd;
case i of
1: dev := garmdev;
2: dev := rarmdev;
end;
getReply; (* send over init command & wait for reply *)
case i of
1: b := bitOn(garmpower);
2: b := bitOn(rarmpower);
end;
b := ok and b;
if not ok then
pp20L('Couldn''t initialize ',20)
else if not b then
pp20L('Power off for ',14);
if not b then begin whichArm; ppOutNow end;
if ok then (* try to calibrate PUMA's *)
begin
while not b do (* get power turned on *)
begin
pp20L('Turn on arm high pow',20); pp20 ('er (Type SPACE to co',20);
pp20 ('ntinue, any other to',20); pp10 (' abort): ',9);
ppOutNow;
ch := getPromptChar;
if ch <> ' ' then (* any letter will abort *)
begin
pp10L(' Aborted ',8);
if not(((i=1) and bitOn(garmcal)) or
((i=2) and bitOn(rarmcal))) then
begin pp20(' - arm not calibrate',20); pp5('d! ',2); ppLine end
else ppLine;
ppOutNow;
b := true; (* so we leave power up loop *)
end
else
begin (* keep trying *)
getReply; (* retry the init command & check power *)
if i = 1 then b := bitOn(garmpower)
else b := bitOn(rarmpower);
b := b and ok;
end;
end;
if ch <> ' ' then b := false
else if i = 1 then b := bitOn(garmcal)
else b := bitOn(rarmcal);
if (ch = ' ') and not b then (* if not already calibrated ... *)
begin
pp20L('Type Y to calibrate ',20); whichArm;
ppOutNow;
ch := getPromptChar;
if (ch = 'Y') then
begin
cmd := calibcmd;
getReply; (* go calibrate arm *)
if ok then pp20L('Calibration complete',20)
else begin pp20L('Error while calibrat',20); pp5('ing ',3); end;
b := ok;
end
else begin pp20L(' Aborted - arm not c',20); pp10('alibrated ',9); end;
ppLine; ppOutNow;
end;
end;
end;
if not b then
begin
pp20L('Type Y to try again:',20); ppchar(' '); ppOutNow;
ch := getPromptChar;
b := (ch <> 'Y');
end
until b;
end;
end;
end;
procedure initArms;
var b: boolean;
begin
(* initMsg(msg,msgp); (* connect to message buffer *)
new(msg); msg↑.ok := true; (* for simulation version *)
(* b := startArm; (* get ARM servo running *)
(* *** *) b := true; (* Someday this will work... *)
if b then
begin
pp20L('Type "Y" to calibrat',20); pp10('e arms: ',8);
ppOutNow;
if getPromptChar = 'Y' then calibrate;
end
else
begin (* Complain if error during startup *)
(* Probably should set some global flag so we don't try to talk to ARM *)
(* or maybe even exit the program *)
pp20L('Error during ARM sta',20); pp20('rtup! Arms not init',20);
pp10('ialized. ',8); ppLine end;
end;
procedure consDef;
begin
xhat := vmake(1,0,0); xhat↑.refcnt := 1000;
yhat := vmake(0,1,0); yhat↑.refcnt := 1000;
zhat := vmake(0,0,1); zhat↑.refcnt := 1000;
nilvect := vmake(0,0,0); nilvect↑.refcnt := 1000;
niltrans := tmake(vsaxwr(zhat,0.0),nilvect); niltrans↑.refcnt := 1000;
(* ypark := tmake(vsaxwr(yhat,180.0),vmake(43.5,2.325,6.86)); *)
bpark := tmake(vsaxwr(yhat,180.0),vmake(43.53125,56.855,9.95875));
gpark := tmake(vsaxwr(zhat,180.0),vmake(83.2,46.13,67.7));
rpark := tmake(niltrans,vmake(84.8,12.87,67.7));
bpark↑.refcnt := 1000;
gpark↑.refcnt := 1000;
rpark↑.refcnt := 1000;
end;
procedure passConstants(var x,y,z,nv: vectorp; var b,g,r,nt: transp);
begin
x := xhat; y := yhat; z := zhat; nv := nilvect;
b := bpark; g := gpark; r := rpark; nt := niltrans;
end;
procedure initWorld;
var v: varidefp; e: enventryp; i,j: integer; envir: environp;
b: boolean;
begin
initArms; (* *** should this go here ??? *** *)
etime := 0;
curtime := 0;
activeInts := nil; (* zero the various queues *)
clkQueue := nil;
readQueue := nil;
allPdbs := nil;
curInt := nil;
allEvents := nil;
resched := false;
singleThreadMode := false;
sysEnv := newEheader; (* set up system variables *)
with sysEnv↑ do
begin
parent := nil;
block := nil;
procp := false;
envir := newEnvironment;
env[0] := envir;
for i := 1 to 4 do env[i] := nil;
end;
i := 0;
j := -1;
v := getsysVars; (* get list of predefined system variables *)
while v <> nil do
begin
(* need to handle devices specially - especially scalar devices *)
e := enterEntry(i,j,envir,sysEnv,v);
b := v↑.offset in [0,2,4,6,8,10,12,16];
(* offsets: arms: 0,4,8 hands: 2,6,10 driver/vise: 12,16 *)
if b then e↑.etype := frametype; (* so we get a frame for scalar devices *)
makeVar(e,v,v↑.tbits); (* make variable environment entry *)
if b then (* set up device values *)
with e↑.f↑ do
begin
ftype := false; (* it's a device *)
sdev := v↑.vtype = svaltype; (* indicate if scalar *)
if sdev then sdest := 0
else
begin
tdest := niltrans;
appr := nil;
depr := nil;
end;
case v↑.offset div 2 of (* set Mechanism bits *)
0: mech := BARMDEV; (* barm *)
1: mech := BHANDDEV; (* bhand *)
2: mech := GARMDEV; (* garm *)
3: mech := GHANDDEV; (* ghand *)
4: mech := RARMDEV; (* rarm *)
5: mech := RHANDDEV; (* rhand *)
6: mech := DRIVERDEV; (* driver *)
8: mech := VISEDEV; (* vise *)
end;
end;
v := v↑.next
end;
for i := j+1 to 9 do envir↑.vals[i] := nil;
speedfactor := getEntry(0,20);
e := getEntry(0,4); (* offset for garm = 4 *)
garm := e↑.f; (* remember frame used for green arm *)
curInt := getPdb;
escInit(escapeI); (* enable escape-I interrupts *)
end;
procedure flushLevel(dLev: integer); (* to clean up from debugger *)
var b: boolean; pri: integer; e: eventp; pp,po: pdbp; ee: enventryp;
begin
pri := dLev * 10;
if curInt <> nil then
if curInt↑.priority >= pri then curInt := nil;
b := true;
while b and (activeInts <> nil) do (* flush run queue *)
if activeInts↑.priority >= pri then activeInts := activeInts↑.next
else b := false;
b := true;
while b and (readQueue <> nil) do (* flush read queue *)
if readQueue↑.priority >= pri then readQueue := readQueue↑.next
else b := false;
e := allEvents;
while e <> nil do
with e↑ do
begin
b := true;
while b and (waitlist <> nil) do (* clean up event's waitlist *)
if waitlist↑.priority >= pri then
begin
waitlist := waitlist↑.next;
count := count + 1;
end
else b := false;
e := next;
end;
po := curInt;
pp := allPdbs;
while pp <> nil do
begin
curInt := pp;
pp := pp↑.nextPdb;
with curInt↑ do
if priority >= pri then (* may need to flush this one *)
begin
killStack;
while level < getELev(env) do killEnv; (* flush envs process created *)
if status = sleepqueue then deClkQueue(curInt);
if cm <> nil then
with cm↑ do
if oldcmon <> nil then
begin
with cmon↑.cdef↑ do ee := getVar(level,offset);
ee↑.c := oldcmon;
freePdb(pdb); (* done with this incarnation of cmon *)
if cmon↑.oncond↑.ntype = forcenode then freeEvent(evt);
relCmoncb(cm);
end
else
begin (* set us up for later *)
priority := (priority mod 10) + 1; (* base level priority again *)
spc := cm↑.cmon;
mode := 0;
status := nullqueue;
running := false;
enabled := false;
end
else
begin
if (not procp) and (evt <> nil) then freeEvent(evt);
freePdb(curInt);
end;
end;
end;
curInt := po;
end;
procedure flushAll(p: pdbp; dLev: integer); (* for use by EDIT *)
var b: boolean; i: integer; e: eventp; pp,po: pdbp;
begin
flushLevel(dLev);
if p <> nil then
begin (* flush process *)
po := curInt;
curInt := p;
with curInt↑ do
begin
killStack;
while level < getELev(env) do killEnv; (* flush envs process created *)
if status = sleepqueue then deClkQueue(curInt);
if cm = nil then relPdb(curInt);
end;
curInt := po;
end;
if dLev = 0 then
begin
etime := 0;
stime := 0;
curtime := 0;
curInt := nil;
activeInts := nil;
readQueue := nil;
resched := false;
(* *** would like to flush any leftover events, unless we saved outermost *** *)
(* *** environment - if we are then we can't.... *** *)
(* while allEvents <> nil do freeEvent(allEvents); (* flush any old events *)
e := allEvents; (* at least we can reset them though *)
while e <> nil do
with e↑ do
begin e↑.waitlist := nil; count := 0; e := next end;
curInt := getPdb;
speedfactor↑.s := 2.0; (* re-initialize speed_factor *)
singleThreadMode := false; (* reset no wait mode *)
(* ??? any other system defined variables need to be reset/reinitialized? ??? *)
end;
end;
procedure unwind(p: pdbp; eLev: integer); (* for use by EDIT *)
var po: pdbp;
begin
po := curInt;
curInt := p;
while eLev < getELev(curInt↑.env) do killEnv; (* unwind inner environments *)
curInt := po;
end;
procedure flushPdb(p: pdbp); (* for use by EDIT *)
var po: pdbp;
begin
if p↑.status = runqueue then
if activeInts = p then activeInts := p↑.next
else
begin
po := activeInts;
while (po↑.next <> nil) and (po↑.next <> p) do po := po↑.next;
if po <> nil then po↑.next := p↑.next;
end
else if p↑.status = inputqueue then
if readQueue = p then readQueue := p↑.next
else
begin
po := readQueue;
while (po↑.next <> nil) and (po↑.next <> p) do po := po↑.next;
if po <> nil then po↑.next := p↑.next;
end;
p↑.priority := 255; (* so we can free just this process using flushLevel *)
flushLevel(25);
end;
procedure flushKids(p: pdbp; zapit: boolean);
var pp: pdbp; b: boolean;
begin
if p↑.status = joinwait then
begin
b := false;
repeat
pp := allPdbs;
repeat (* find one of the threads *)
with pp↑ do
if (not procp) and (cm = nil) and (evt <> nil) then
if evt↑.waitlist = p then
begin flushKids(pp,true); pp := nil end; (* flush it *)
if pp <> nil then (* move on to next *)
begin pp := pp↑.nextPdb; b := pp = nil end;
until pp = nil;
until b; (* repeat til we find all of them *)
end
else if p↑.status = proccall then
begin
pp := allPdbs;
repeat
if pp↑.procp and (pp↑.opdb = p) then
begin flushKids(pp,true); pp := nil end (* flush it *)
else pp := pp↑.nextPdb;
until pp = nil;
end;
if zapit then flushPdb(p);
end;
(* aux routines: cmonEnable, cmonDisable, cmonCheck *)
procedure cmonEnable(e: enventryp);
var p: pdbp; b: boolean; pri: integer;
begin
with e↑.c↑ do
if (enabled or running) and ((pdb↑.priority mod 10) < debugLevel) then
makeCmon(e,cmon↑.cdef); (* push old & make another for this debug level *)
with e↑.c↑ do
begin
pdb↑.mech := curInt↑.mech; (* inherit device being controlled *)
if running then enabled := true (* if currently running, re-enable it *)
else if not enabled then (* is it currently enabled? *)
begin
enabled := true; (* now it is *)
pdb↑.status := runqueue;
pdb↑.priority := (pdb↑.priority mod 10) + (10 * debuglevel);
addPdb(activeInts,pdb); (* add cmon to list of active processes *)
if pdb↑.priority > curInt↑.priority then
resched := true; (* need to swap us out *)
end;
end;
end;
procedure cmonDisable(c: cmoncbp);
var p,pp: pdbp; b: boolean; n,np: nodep;
begin
with c↑ do
begin
if enabled then (* is it currently enabled? *)
begin
enabled := false; (* disable it *)
if cmon↑.oncond↑.ntype = forcenode then
begin
with msg↑ do
begin
cmd := forceoffcmd;
bits := fbits;
evt := c↑.evt;
end;
sendCmd; (* tell force system to stop checking for this force condition *)
end;
if cmon↑.exprCm or (cmon↑.oncond↑.ntype = durnode) then deClkQueue(pdb)
else
begin (* remove pdb from event queue *)
p := evt↑.waitlist;
pp := nil;
while (p <> nil) and (p <> pdb) do begin pp := p; p := p↑.next end;
if p <> nil then (* found us, now splice us out of the list *)
if pp = nil then evt↑.waitlist := p↑.next else pp↑.next := p↑.next;
end;
pdb↑.next := nil;
end;
end;
end;
function cmonCheck: boolean;
var b: boolean; i: integer; env: environp; ev: enventryp;
begin (* make sure all cmon's in current environment have finished *)
b := true;
env := curInt↑.env↑.env[0]; (* point to first environment record *)
i := 0;
ev := env↑.vals[0];
while (ev <> nil) and b do
with ev↑ do
begin (* see if any cmons are running *)
if etype = cmontype then
begin (* found a cmon *)
if c↑.running then
b := c↑.pdb↑.priority >= curInt↑.priority (* is it running now? *)
else cmonDisable(c); (* if not disabled it *)
end;
i := i + 1;
if i <= 9 then ev := env↑.vals[i]
else
begin
i := 0;
env := env↑.next; (* use next env record *)
if env <> nil then ev := env↑.vals[0] else ev := nil;
end;
end;
cmonCheck := b; (* true if no cmons are now running *)
end;
(* expression evaluator: evalExp *)
procedure evalExp;
var res, n1, n2, n3: nodep; p: pdbp; i, j, tbits: integer; vfp: varidefp;
ep,epar: enventryp; envir: environp; envhdr: envheaderp; ch: ascii;
b, b1, b2, b3: boolean;
begin
with curInt↑.epc↑ do
begin
if ntype = leafnode then
if ltype = varitype then with vari↑ do getVal(level, offset)
else begin (* should only get here for constants, badops & subscripts *)
if ltype = pconstype then n1 := pcval else n1 := curInt↑.epc;
res:= newNode;
with res↑ do
begin
ntype := leafnode;
ltype := n1↑.ltype;
length := n1↑.length; (* this should work for all leaftypes *)
str := n1↑.str;
end;
push(res);
end
else if ntype = exprnode then
begin
n2 := nil; b2 := false;
n3 := nil; b3 := false;
if (op < ioop) or (op = adcop) or (op = dacop) then (* not a special op *)
begin (* pop appropriate number of args off of stack *)
n1 := getNval(arg1,b1); (* all ops have at least one arg *)
if arg2 <> nil then
begin
n2 := getNval(arg2,b2);
if arg3 <> nil then
begin
n3 := getNval(arg3,b3);
end;
end
end
else begin n1 := nil; b1 := false end;
if (op < specop) or (op = adcop) then (* make sure it's not a special op *)
begin
res := newNode;
res↑.ntype := leafnode;
if (op < vecop) or (ioop < op) then res↑.ltype := svaltype
else if op < transop then res↑.ltype := vectype
else res↑.ltype := transtype;
end;
case op of (* assumes correct args on stack *)
(* relations *)
sltop: if n1↑.s < n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sleop: if n1↑.s <= n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
seqop: if n1↑.s = n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sgeop: if n1↑.s >= n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sgtop: if n1↑.s > n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sneop: if n1↑.s <> n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
(* logical *)
notop: if n1↑.s = 0.0 then res↑.s := 1.0 else res↑.s := 0.0;
orop: if (n1↑.s <> 0) or (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
xorop: if (n1↑.s <> 0) <> (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
andop: if (n1↑.s <> 0) and (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
eqvop: if (n1↑.s <> 0) = (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
(* scalar ops *)
saddop: res↑.s := n1↑.s + n2↑.s;
ssubop: res↑.s := n1↑.s - n2↑.s;
smulop: res↑.s := n1↑.s * n2↑.s;
sdivop: res↑.s := n1↑.s / n2↑.s;
snegop: res↑.s := - n1↑.s;
sabsop: res↑.s := abs(n1↑.s);
sexpop: res↑.s := exp(n2↑.s * ln(n1↑.s));
maxop: if n1↑.s > n2↑.s then res↑.s := n1↑.s else res↑.s := n2↑.s;
minop: if n1↑.s < n2↑.s then res↑.s := n1↑.s else res↑.s := n2↑.s;
intop: res↑.s := round(n1↑.s);
idivop: res↑.s := round(n1↑.s) div round(n2↑.s);
modop: res↑.s := round(n1↑.s) mod round(n2↑.s);
(* functions *)
sqrtop: res↑.s := sqrt(n1↑.s);
logop: res↑.s := ln(n1↑.s);
expop: res↑.s := exp(n1↑.s);
timeop: res↑.s := curtime - n1↑.s; (* ** daytime? conversion to secs? ** *)
(* trig *)
sinop: res↑.s := sind(n1↑.s);
cosop: res↑.s := cosd(n1↑.s);
tanop: res↑.s := tand(n1↑.s);
asinop: res↑.s := asin(n1↑.s);
acosop: res↑.s := acos(n1↑.s);
atan2op: res↑.s := atan2(n1↑.s,n2↑.s);
(* vector ops *)
vdotop: res↑.s := vdot(n1↑.v,n2↑.v);
vmagnop: res↑.s := vmagn(n1↑.v);
unitvop: res↑.v := unitv(n1↑.v);
vaddop: res↑.v := vadd(n1↑.v,n2↑.v);
vsubop: res↑.v := vsub(n1↑.v,n2↑.v);
vnegop: res↑.v := svmul(-1.0,n1↑.v);
crossvop: res↑.v := vcross(n1↑.v,n2↑.v);
vmakeop: res↑.v := vmake(n1↑.s,n2↑.s,n3↑.s);
svmulop: res↑.v := svmul(n1↑.s,n2↑.v);
vsmulop: res↑.v := svmul(n2↑.s,n1↑.v);
vsdivop: res↑.v := vsdiv(n1↑.v,n2↑.s);
tvmulop: res↑.v := tvmul(n1↑.t,n2↑.v);
wrtop: res↑.v := tvmul(torient(n2↑.t),n1↑.v);
(* trans ops *)
tposop: res↑.v := tpos(n1↑.t);
taxisop: res↑.v := taxis(n1↑.t);
tmagnop: res↑.s := tmagn(n1↑.t);
fmakeop,
tmakeop: res↑.t := tmake(n1↑.t,n2↑.v);
torientop: res↑.t := torient(n1↑.t);
ttmulop: res↑.t := ttmul(n1↑.t,n2↑.t);
tvaddop: res↑.t := tvadd(n1↑.t,n2↑.v);
tvsubop: res↑.t := tvsub(n1↑.t,n2↑.v);
tinvrtop: res↑.t := tinvrt(n1↑.t);
vsaxwrop: res↑.t := vsaxwr(n1↑.v,n2↑.s);
constrop: res↑.t := construct(n1↑.v,n2↑.v,n3↑.v);
ftofop: res↑.t := ttmul(tinvrt(n1↑.t),n2↑.t);
vmkfrcop: res↑.t := vmkfrc(n1↑.v);
(* input ops *)
queryop: begin (* now print everything out *)
b := false;
if not inputReady then
if readQueue = nil then
begin (* first time through *)
prntplist(arg2);
b := true;
end
else if (readQueue↑.priority div 10) < (curInt↑.priority div 10) then
begin (* first time through *)
prntplist(arg2);
b := true;
end
else sleep(60) (* wait a sec for other input to finish *)
else
begin
inputReady := false;
ch := inputLine[1];
if ord(ch) >= smallA then
ch := chr(ord(ch) - smallA + ord('A')); (* make upper case *)
if (ch = 'Y') or (ch = 'N') then
begin
if ch = 'Y' then res↑.s := 1.0 else res↑.s := 0.0;
push(res);
end
else b := true; (* ask again *)
end;
if b then
begin
relNode(res);
pp20L('Type Y or N: ',13);
ppOutNow;
curInt↑.next := readQueue;
readQueue := curInt; (* swap us out *)
curInt↑.status := inputqueue;
curInt := nil;
inputp := 0;
resched := true;
end
end;
inscalarop: begin
if not inputReady then
begin
if readQueue = nil then b := true
else b := (readQueue↑.priority div 10)<(curInt↑.priority div 10);
if b then
begin (* first time through *)
pp20L('Scalar please: ',15); ppOutNow;
curInt↑.next := readQueue;
readQueue := curInt; (* swap us out *)
curInt↑.status := inputqueue;
curInt := nil;
inputp := 0;
resched := true;
end
else sleep(60); (* wait a sec for other input to finish *)
relNode(res);
end
else
begin (* parse the number *)
inputReady := false;
b := true; (* assume plus *)
i := 1;
while (i <= inputp) and (inputLine[i] = ' ') do i := i + 1;
if inputLine[i] = '+' then i := i + 1
else if inputLine[i] = '-' then begin b := false; i := i + 1 end;
while (i <= inputp) and (inputLine[i] = ' ') do i := i + 1;
j := 0;
while (i <= inputp) and (* get integer part *)
('0' <= inputLine[i]) and (inputLine[i] <= '9') do
begin j := 10*j + ord(inputLine[i]) - ord('0'); i := i + 1 end;
res↑.s := j;
if inputLine[i] = '.' then
begin (* get fractional part *)
i := i + 1;
j := 10;
while (i <= inputp) and
('0' <= inputLine[i]) and (inputLine[i] <= '9') do
begin
res↑.s := res↑.s + (ord(inputLine[i]) - ord('0')) / j;
j := 10 * j;
i := i + 1;
end;
end;
if not b then res↑.s := - res↑.s;
push(res);
end;
end;
vmop: ;
adcop: with msg↑ do
begin
cmd := readadccmd;
n := round(n1↑.s); (* get channel # *)
if (n < 0) or (63 < n) then (* bad channel # *)
begin
pp20L('A/D channel out of r',20); pp20('ange - using chan 0 ',19);
ppLine;
n := 0;
end;
getReply; (* have ARM servo read it in *)
res↑.s := val; (* store result away *)
end;
dacop: with msg↑ do
begin
cmd := writedaccmd;
n := round(n1↑.s); (* get channel # *)
if (n < 1) or (4 < n) then (* bad channel # *)
begin
pp20L('D/A channel out of r',20); pp20('ange - using chan 1 ',19);
ppLine;
n := 1;
end;
val := n2↑.s; (* & magnitude *)
sendCmd; (* have ARM servo write it out *)
end;
(* special *)
arefop: with arg1↑.vari↑ do getVal(level,offset); (* should never get here *)
callop: begin
p := getPdb;
with p↑ do
begin
opdb := curInt;
procp := true;
status := nowrunning;
pdef := arg1↑.vari↑.p;
level := pdef↑.level;
spc := pdef↑.body; (* code to execute *)
end;
with arg1↑.vari↑ do
ep := getVar(level, offset); (* environment entry for procedure *)
envhdr := newEheader;
p↑.env := envhdr;
with envhdr↑ do
begin
parent := ep↑.penv; (* parent is env where proc defined *)
procp := true;
proc := ep↑.p;
varcnt := 0;
for j := 1 to 4 do env[j] := nil;
end;
vfp := ep↑.p↑.paramlist; (* formal parameters *)
n1 := arg2; (* actual parameters *)
envir := newEnvironment; (* always need at least one environment record *)
envir↑.next := nil;
envhdr↑.env[0] := envir;
for j := 0 to 9 do envir↑.vals[j] := nil;
i := 0;
j := -1;
while vfp <> nil do (* make parameter variables *)
begin
epar := enterEntry(i,j,envir,envhdr,vfp);
tbits := vfp↑.tbits;
if tbits = 4 then (* call by reference *)
with n1↑.lval↑ do
if ((ntype = exprnode) and (op <> arefop)) or (* expression *)
((ntype = leafnode) and (ltype <> varitype)) (* constant *)
then tbits := 0; (* change to call by value *)
makeVar(epar,vfp,tbits); (* make var's environment entry *)
with n1↑.lval↑ do (* now bind actual parameter value *)
if tbits = 5 then (* array passed by reference *)
with vari↑ do epar↑.r := getEntry(level,offset)
else if tbits = 4 then (* regular variable passed by reference *)
epar↑.r := gtVarn(n1↑.lval)
else (* need to copy value *)
begin
n2 := getNval(n1↑.lval,b);
with epar↑ do
case etype of
svaltype: s := n2↑.s;
vectype,
transtype: begin
v := n2↑.v;
v↑.refcnt := v↑.refcnt + 1;
end;
frametype: begin
f↑.val := n2↑.t;
f↑.valid := 0; (* mark us as valid *)
f↑.val↑.refcnt := f↑.val↑.refcnt + 1;
end;
strngtype: begin length := n2↑.length; str := n2↑.str end;
end;
if b then killNode(n2); (* done with stack entry *)
end;
n1 := n1↑.next;
vfp := vfp↑.next;
end;
for i := j+1 to 9 do envir↑.vals[i] := nil;
curInt↑.epc := curInt↑.epc↑.next; (* advance our epc now *)
curInt↑.status := proccall;
curInt := p; (* swap to procedure now *)
end;
badop: ;
end;
if op < ioop then push(res); (* save result on stack *)
if b1 then relNode(n1); (* release nodes when done with them *)
if b2 then relNode(n2);
if b3 then relNode(n3);
end
else if ntype <> listnode then
begin (* **** error - bad node **** *)
pp20L('Error in Eval - bad ',20); pp10('node type ',9); ppLine;
(* code to recover??? *)
end;
end;
if curInt <> nil then (* in case we're now waiting for input *)
with curInt↑ do (* advance pointer to next node to be evaluated *)
if epc <> nil then epc := epc↑.next;
end;
procedure doProg; (* ** ** *)
begin
(* *** stuff to reset affixments *** *)
speedfactor↑.s := 2.0; (* initialize speed_factor *)
garm↑.tdest := gpark; (* for 10 version *)
curInt↑.spc := curInt↑.spc↑.pcode;
curInt↑.mode := 0;
end;
procedure doBlock;
var i,j: integer; v: varidefp;
envhdr: envheaderp; e: enventryp; envir: environp;
begin
with curInt↑ do
begin
if spc↑.variables <> nil then
with spc↑ do
begin
envhdr := newEheader;
envhdr↑.parent := env;
env := envhdr;
envhdr↑.block := spc;
envhdr↑.varcnt := 0;
envhdr↑.procp := false;
envir := newEnvironment; (* always need at least one environment record *)
envir↑.next := nil;
envhdr↑.env[0] := envir;
for j := 1 to 4 do envhdr↑.env[j] := nil;
for j := 0 to 9 do envir↑.vals[j] := nil;
i := 0;
j := -1;
v := variables;
while v <> nil do
begin
if v↑.vtype < dimensiontype then
begin
e := enterEntry(i,j,envir,envhdr,v);
makeVar(e,v,v↑.tbits); (* make variable environment entry *)
end
else (* if v↑.vtype = freevartype then - need to do it for macros too *)
begin
relEentry(enterEntry(i,j,envir,envhdr,v)); (* space past env entry *)
envir↑.vals[j] := nil;
end;
v := v↑.next
end;
for i := j+1 to 9 do envir↑.vals[i] := nil;
end;
mode := 0;
spc := spc↑.bcode;
end;
end;
procedure doCoblock;
var e: eventp;
procedure sched(n: nodep);
var p: pdbp;
begin
if n↑.next <> nil then sched(n↑.next); (* maintain lexical order *)
if n↑.cstmnt↑.stype <> commenttype then
begin (* we don't want to schedule comments (yet) *)
p := getPdb; (* get a pdb for this thread *)
with p↑ do
begin
next := activeInts; (* add us to list of active interpreters *)
activeInts := p;
status := runqueue;
spc := n↑.cstmnt;
sdef := spc;
evt := e; (* event to signal when we go away *)
end;
end;
end;
begin
with curInt↑ do
case mode of
1: begin (* schedule the parallel threads for execution *)
mode := 2;
if spc↑.threads <> nil then
begin
e := getEvent; (* event to use for signalling when all threads are done *)
e↑.count := -spc↑.nthreads;
e↑.waitlist := curInt;
sched(spc↑.threads); (* schedule all the threads *)
curInt↑.status := joinwait;
curInt := nil;
resched := true; (* start up first of them *)
end;
end;
2: begin (* all threads are done - continue with main *)
mode := 0;
spc := spc↑.next;
end;
end;
end;
procedure doEnd;
var spcp: statementp; e: eventp; b: boolean;
begin
b := true;
with curInt↑ do
begin
spcp := spc↑.bparent;
case spcp↑.stype of
progtype: begin
running := false; (* all done running *)
mode := 0;
end;
blocktype: begin
if spcp↑.variables <> nil then (* any variables? *)
b := cmonCheck; (* any cmons now running? *)
if b then
begin (* no - we can clean things up *)
if spcp↑.variables <> nil then killEnv;
spcp := spcp↑.next;
mode := 0;
end
else sleep(30); (* give cmons time to finish *)
end;
coblocktype: begin
if evt = nil then
begin
running := false; (* break to debugger *)
(* *** if not singleThreadMode then complain??? *** *)
end
else
begin
b := false;
e := evt;
killStack; (* flush stack *)
freePdb(curInt);
if e↑.count = -1 then
begin (* this was last thread *)
curInt := e↑.waitlist; (* return to main *)
curInt↑.status := nowrunning;
freeEvent(e);
if activeInts <> nil then
if curInt↑.priority < activeInts↑.priority then
resched := true;
end
else
begin (* other threads still executing *)
e↑.count := e↑.count + 1;
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
end;
cmtype: begin (* terminate or resched this cmon *);
cm↑.running := false;
killStack;
b := false;
spc := spcp; (* set us up for next time *)
mode := 0;
if not cm↑.enabled then
begin (* we're done, swap us out *)
curInt↑.status := nullqueue;
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
fortype: begin
if sp↑.ntype <> forvalnode then (* gack! stack error *)
begin
pp20L('Can''t find FOR node ',20); pp20('- stack error!!! ',16);
ppLine;
(* could try to recover, but.... *)
end;
sp↑.fvar↑.s := sp↑.fvar↑.s + sp↑.fstep; (* next for value *)
mode := 2; (* do for check *)
end;
untiltype: mode := 2;
whiletype: mode := 0;
movetype, (* for error handler *)
iftype,
casetype: begin
spcp := spcp↑.next;
mode := 0;
end;
end;
if b then spc := spcp;
end;
end;
procedure doFor;
var ev: enventryp; fnode, res: nodep;
begin
with curInt↑ do
case mode of
1: begin (* stack contains: forvar subscripts, initial, step & final values *)
ev := gtVarn(spc↑.forvar); (* access variable *)
res := pop; (* get initial value *)
ev↑.s := res↑.s; (* store it away *)
relNode(res); (* release node *)
fnode := sp; (* get step value *)
fnode↑.ntype := forvalnode;
fnode↑.fstep := fnode↑.s; (* copy step value - note s & step fields may overlap *)
fnode↑.fvar := ev; (* copy environment entry *)
mode := 2;
end;
2: begin
fnode := sp;
if (fnode↑.fvar↑.s - fnode↑.next↑.s) * fnode↑.fstep <= 0.0 (* (cur-fin)*step *)
then spc:= spc↑.fbody (* go interpret for body *)
else begin
spc := spc↑.next; (* move on to next statement *);
res := fnode↑.next;
sp := res↑.next; (* pop for nodes off of stack *)
relNode(fnode); (* and release them *)
relNode(res);
end;
mode := 0;
end;
end;
end;
procedure doIf;
var res: nodep; s: statementp;
begin
with curInt↑ do
begin
res := pop; (* pop value off of stack *)
s := spc;
if res↑.s = 0.0 then spc := s↑.els else spc := s↑.thn;
if spc = nil then spc := s↑.next; (* if nil clause just go on to next stmnt *)
relNode(res);
mode := 0;
end;
end;
procedure doWhile;
var res: nodep;
begin
with curInt↑ do
begin
res := pop; (* pop value off of stack *)
if res↑.s = 0.0 then spc := spc↑.next (* if false move on to next stmnt *)
else if spc↑.body <> nil then spc := spc↑.body;
relNode(res);
mode := 0;
end;
end;
procedure doUntil;
var res: nodep;
begin
with curInt↑ do
case mode of
1: begin
if spc↑.body <> nil then begin spc := spc↑.body; mode := 0 end
else mode := 2;
end;
2: begin
epc := spc↑.exprs; (* need to evaluate until condition *)
mode := 3;
end;
3: begin
res := pop; (* pop value off of stack *)
if (res↑.s <> 0.0) then
begin
spc := spc↑.next; (* if true move on to next stmnt *)
mode := 0;
end
else mode := 1; (* if still false repeat body *)
relNode(res);
end;
end;
end;
procedure doCase;
var i: integer; p: nodep; spcp: statementp; b: boolean;
begin
with curInt↑ do
begin
p := pop; (* pop index value off of stack *)
i := round(p↑.s);
relNode(p);
spcp := nil;
p := spc↑.caselist;
if (i >= 0) and (i <= abs(spc↑.range)) then (* index within range *)
begin (* try to find proper case *)
b := true;
while (p <> nil) and b do
if (p↑.cval = i) then b := false else p := p↑.next;
if p <> nil then
begin spcp := p↑.stmnt; if spcp = nil then spcp := spc↑.next end
else if spc↑.range >= 0 then spcp := spc↑.next (* null statement *)
end;
if (spcp = nil) and (spc↑.range < 0) then
begin (* if none found and it's a labelled case statement check for else *)
p := spc↑.caselist;
b := true;
while (p <> nil) and b do (* search for else stmnt *)
if (p↑.cval = -1) then b := false else p := p↑.next;
if p <> nil then spcp := p↑.stmnt
end;
if spcp = nil then
begin
pp20L('Case index out of ra',20); pp5('nge: ',5); ppInt(i); ppLine;
spcp := spc↑.next;
end;
spc := spcp;
mode := 0;
end;
end;
procedure doCall;
var n: nodep;
begin
with curInt↑ do
begin
if spc↑.what↑.arg1↑.vari↑.vtype <> nulltype then (* flush unused result *)
n := pop;
mode := 0;
spc := spc↑.next; (* move on to next statement *);
end;
end;
procedure doReturn;
var p: pdbp; n: nodep; b,debRet: boolean;
begin
b := true;
with curInt↑ do
begin
if procp then debRet := false (* normal case *)
else if (priority > 9) and (nextpdb = nil) and (opdb <> nil) then
debRet := true (* immediately executed RETURN *)
else b := false; (* no good - nothing to return from *)
if debRet then p := opdb↑.opdb else p := opdb; (* pdb of caller *)
if b then
begin
while b and not env↑.procp do
begin (* make sure all cmon's in outer environments have finished *)
b := cmonCheck;
if b then killEnv; (* flush all environments out to parameters *)
end;
if b then (* no cmons now running *)
begin (* now we can clean things up & return from the procedure *)
if spc↑.retval <> nil then n := pop (* get return value *)
else n := nil;
if env↑.proc↑.ptype <> nulltype then
begin (* yes - put return value on caller's stack *)
if n <> nil then
if env↑.proc↑.ptype <> n↑.ltype then
begin
killNode(n);
n := nil;
end;
if n = nil then
begin
n := newNode;
with n↑ do (* use default value *)
begin
ntype := leafnode;
ltype := env↑.proc↑.ptype; (* copy datatype of result *)
if ltype = svaltype then s := 0.0 (* it's a scalar *)
else if ltype = vectype then v := nilvect
else if ltype = transtype then t := niltrans
else begin length := 0; str := nil end;
end;
end;
n↑.next := p↑.sp;
p↑.sp := n;
end;
killEnv; (* flush procedure's parameters too *)
killStack; (* flush stack *)
if debRet then
begin
opdb↑.opdb↑.status := runqueue;
addPdb(activeInts,opdb↑.opdb); (* re-activate caller *)
opdb↑.level := 255; (* so we don't re-release environments *)
flushKids(opdb,true); (* flush old procedure's pdb *)
spc := sdef↑.next; (* point to our abort *)
running := false; (* and return to debugger *)
end
else
begin
freePdb(curInt); (* flush procedure's pdb *)
curInt := p; (* all done - return *)
curInt↑.status := nowrunning;
end;
end
else sleep(30); (* give cmons time to finish *)
end
else
begin
pp20L('Ignoring return ',16); ppLine;
if spc↑.retval <> nil then n := pop; (* flush return value *)
spc := spc↑.next; (* just move on to next statement *)
mode := 0;
end;
end;
end;
procedure doPrint;
begin
with curInt↑ do
begin (* print everything out *)
prntplist(spc↑.plist);
mode := 0;
spc := spc↑.next;
end;
end;
procedure doPrompt;
const smallP = 112; (* Lowercase p *)
var ch: ascii; b: boolean;
begin
with curInt↑ do
case mode of
1: begin
if readQueue = nil then b := true
else b := (readQueue↑.priority div 10) < (curInt↑.priority div 10);
if b then
begin (* first time through *)
prntplist(spc↑.plist);
mode := 2;
end
else sleep(60) (* wait a sec for other input to finish *)
end;
2: begin
pp20L('Type P to proceed: ',19);
ppOutNow;
mode := 3;
curInt↑.next := readQueue;
readQueue := curInt; (* swap us out *)
curInt↑.status := inputqueue;
curInt := nil;
inputp := 0;
resched := true;
end;
3: begin
inputReady := false;
if (inputLine[1] = chr(smallP)) or (inputLine[1] = 'P') then
begin
mode := 0;
spc := spc↑.next;
end
else mode := 2; (* try again *)
end;
end;
end;
procedure doPause;
var i: integer; n: nodep;
begin
n := pop;
i := round(n↑.s * 60); (* get pause time (in 60Hz ticks) *)
relNode(n);
curInt↑.mode := 0; (* get ready for next statement *)
curInt↑.spc := curInt↑.spc↑.next;
sleep(i); (* put us to sleep for a while *)
end;
procedure doAbort;
begin
with curInt↑ do
begin (* print everything out *)
if spc↑.debugLev = 0 then
begin (* a real abort *)
(* tell arm servo to abort all motions in progress *)
(*
{$C .MCALL SETF$S
SETF$S #40. ;Signal Aborts by setting common event flag
}
*)
(* msg↑.cmd := abortcmd; *) (* latter we'll do it with messages *)
(* sendCmd; *)
prntplist(spc↑.plist);
spc := spc↑.next;
pp10L('Aborting ',8);
running := false; (* break to debugger *)
end
else if debugLevel = spc↑.debugLev then
running := false (* break if debugger process *)
else spc := spc↑.next; (* just ignore it *)
mode := 0;
end;
end;
procedure doAssign;
var ev: enventryp; res: nodep;
begin
with curInt↑.spc↑.what↑ do
begin
if ntype = leafnode then
with vari↑ do setVal(level,offset) (* store into simple variable *)
else
case op of (* see what type of store we're to do *)
arefop: with arg1↑.vari↑ do setVal(level,offset); (* store into array var *)
deproachop: begin (* any subscripts & deproach value on stack *)
ev := gtVarn(curInt↑.spc↑.what); (* access variable *)
res := pop; (* get deproach value *)
(* check we've really got a frame? *)
ev↑.f↑.fdepr := res↑.t; (* store it away *)
relNode(res);
end;
tposop,
torientop: begin
with arg1↑ do
if ntype = leafnode then
with vari↑ do setVal(level,offset) (* simple variable *)
else
with arg1↑.vari↑ do setVal(level,offset); (* array variable *)
end;
end;
curInt↑.mode := 0;
curInt↑.spc := curInt↑.spc↑.next; (* move on to next statement *);
end;
end;
procedure doSignal;
var ev: enventryp; p, pt: pdbp; st: statementp;
begin
with curInt↑ do
begin
st := spc;
spc := spc↑.next; (* advance our pc now before possibly swapping ourself out *)
mode := 0;
if singleThreadMode then
begin
pp20L('Would signal event: ',20); prntVar(st↑.event);
end
else if st↑.event <> nil then
begin
ev := gtVarn(st↑.event); (* access variable *)
ev↑.evt↑.count := ev↑.evt↑.count + 1;
p := ev↑.evt↑.waitlist; (* get pdb of process to schedule (if any) *)
if p <> nil then
begin
ev↑.evt↑.waitlist := p↑.next; (* remove node from waitlist *)
if p↑.priority > priority then
begin (* swap it in and swap us out *)
p↑.status := nowrunning;
pt := curInt;
curInt := p;
p := pt;
end;
p↑.status := runqueue;
addPdb(activeInts,p); (* add whoever to active process list *)
end;
end;
end;
end;
procedure doWait;
var ev: enventryp; p: pdbp; st: statementp; b: boolean;
begin
with curInt↑ do
if singleThreadMode then
if mode = 1 then
begin
if readQueue = nil then b := true
else b := (readQueue↑.priority div 10) < (curInt↑.priority div 10);
if b then
begin (* first time through *)
pp20L('Would wait for event',20); pp5(': ',2); prntVar(spc↑.event);
mode := 2;
doPrompt; (* now have user type a "P" to proceed *)
end
else sleep(60) (* wait a sec for other input to finish *)
end
else doPrompt
else
begin
st := spc;
spc := spc↑.next; (* advance our pc now before maybe swapping out *)
mode := 0;
if st↑.event <> nil then
begin
ev := gtVarn(st↑.event); (* access variable *)
ev↑.evt↑.count := ev↑.evt↑.count - 1;
if ev↑.evt↑.count < 0 then (* hasn't been signalled yet, need to wait *)
begin
curInt↑.status := eventqueue;
addPdb(ev↑.evt↑.waitlist,curInt); (* add us to wait list *)
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
end;
end;
procedure doEnable;
begin
with curInt↑ do
begin
if spc↑.cmonlab = nil then
if cm <> nil then cm↑.enabled := true (* re-enabling this cmon *)
else
begin
pp20L('No cmon to enable! ',18); ppLine;
end
else
begin
with spc↑.cmonlab↑.s↑.cdef↑ do
cmonEnable(getVar(level,offset)); (* enable cmon control block *)
end;
mode := 0;
spc := spc↑.next;
end;
end;
procedure doDisable;
var e: enventryp;
begin
with curInt↑ do
begin
if spc↑.cmonlab = nil then
if cm <> nil then cm↑.enabled := false (* disabling this cmon *)
else
begin
pp20L('No cmon to disable! ',19); ppLine;
end
else
begin
with spc↑.cmonlab↑.s↑.cdef↑ do
e := getVar(level,offset); (* get cmon control block *)
if e↑.c↑.running then sleep(30) (* if running wait for it to finish *)
else
begin
cmonDisable(e↑.c); (* disable it *)
mode := 0;
spc := spc↑.next;
end;
end;
end;
end;
(* affixment auxiliary routines: affixaux, unfixaux & unfix *)
procedure affixaux (f, d: framep; cnt: integer);
var c1,c2,ct: nodep;
begin
with f↑ do
if not (ftype and (dev <> nil)) then (* haven't marked it yet *)
begin
if not ftype then cnt := 1 (* it's a device *)
else begin dev := d; dcntr := cnt; cnt := cnt + 1; end; (* mark frame *)
c1 := calcs;
ct := nil;
while c1 <> nil do
begin (* mark everyone it's affixed to *)
if c1↑.rigid or not c1↑.frame1 then affixaux(c1↑.other,d,cnt)
else if c1↑.other↑.dev = nil then
begin (* need to break non-rigid affixment *)
(* first splice calcs out of affixment lists *)
if ct = nil then calcs := c1↑.next else ct↑.next := c1↑.next;
c2 := c1↑.other↑.calcs;
ct := nil;
while c2↑.other <> f do begin ct := c2; c2 := c2↑.next; end;
if ct = nil then c1↑.other↑.calcs := c2↑.next else ct↑.next := c2↑.next;
if not c1↑.tvarp then
begin (* release relation trans *)
upTrans(c1↑.tval,nil);
upTrans(c2↑.tval,nil);
end;
relNode(c1); (* finally release calc nodes *)
relNode(c2);
c1 := ct;
end;
ct := c1;
c1 := c1↑.next;
end;
end;
end;
function unfixaux (f: framep; cnt: integer): boolean;
var c: nodep; b: boolean; d: framep;
begin
b := false;
with f↑ do
if not ftype then affixaux(f,f,1) (* a device - remark everyone as dynamic *)
else if dev <> nil then (* check we're still marked as dynamic, else done *)
if cnt > dcntr then
begin
d := dev; dev := nil; (* so affixaux will mark us *)
affixaux(f,d,dcntr); (* need to remark everyone *)
end
else
begin (* unmark us *)
dev := nil;
dcntr := 0;
b := true;
c := calcs;
while (c <> nil) and b do
begin
b := unfixaux(c↑.other,cnt);
c := c↑.next
end
end;
unfixaux := b;
end;
procedure unfix (* f1,f2: framep *);
var t: transp; c1, c2: nodep; b: boolean; i: integer;
begin
if f1↑.ftype then t := feval(f1); (* try to get a value for both *)
if f2↑.ftype then t := feval(f2); (* if they're frames *)
c1 := f1↑.calcs; (* unfix f1 from f2 *)
c2 := nil;
b := true;
while (c1 <> nil) and b do
if c1↑.other = f2 then
begin (* found calc - splice it out of list *)
b := false;
if c2 = nil then f1↑.calcs := c1↑.next else c2↑.next := c1↑.next;
if not c1↑.tvarp then upTrans(c1↑.tval,nil); (* release old trans values *)
relNode(c1); (* done with calc node *)
end
else begin c2 := c1; c1 := c1↑.next end; (* try next *)
c1 := f2↑.calcs; (* now unfix f2 from f1 *)
c2 := nil;
b := true;
while (c1 <> nil) and b do
if c1↑.other = f1 then
begin (* found calc - splice it out of list *)
b := false;
if c2 = nil then f2↑.calcs := c1↑.next else c2↑.next := c1↑.next;
if not c1↑.tvarp then upTrans(c1↑.tval,nil); (* release old trans values *)
relNode(c1); (* done with calc node *)
end
else begin c2 := c1; c1 := c1↑.next end; (* try next *)
if not f1↑.ftype then b := unfixaux(f2,0) (* f2 no longer dynamic *)
else if not f2↑.ftype then b := unfixaux(f1,0) (* f1 no longer dynamic *)
else if f1↑.dev <> nil then (* both currently dynamic *)
if f1↑.dcntr < f2↑.dcntr then b := unfixaux(f2,f1↑.dcntr) (* unmark f2 *)
else b := unfixaux(f1,f2↑.dcntr); (* unmark f1 *)
end;
procedure doAffix;
var f1, f2: framep; ev: enventryp; c1, c2: nodep; t: transp; b: boolean;
begin
with curInt↑ do
begin (* stack has subscripts for frame1, frame2 & byvar & atexp value *)
ev := gtVarn(spc↑.frame1); (* access variable *)
f1 := ev↑.f;
ev := gtVarn(spc↑.frame2); (* access variable *)
f2 := ev↑.f;
if spc↑.byvar <> nil then
ev := gtVarn(spc↑.byvar) (* access variable *)
else ev := nil;
if spc↑.atexp <> nil then
begin
c1 := pop; (* get at expression value *)
t := c1↑.t; (* save it for later *)
relNode(c1); (* release node *)
end
else t := ttmul(feval(f1),tinvrt(feval(f2))); (* need to compute it *)
c1 := f1↑.calcs; (* see if frames are already affixed *)
b := true;
while b and (c1 <> nil) do
if c1↑.other = f2 then b := false else c1 := c1↑.next;
if c1 <> nil then (* currently affixed *)
begin
c2 := f2↑.calcs; (* find its mate *)
while c2↑.other <> f1 do c2 := c2↑.next;
if (not c1↑.tvarp) and (spc↑.byvar <> nil) then
begin (* if old affixment was direct and new one isn't *)
upTrans(c1↑.tval,nil); (* release old trans values *)
upTrans(c2↑.tval,nil);
end;
end
else
begin (* get a pair of calc nodes *)
c1 := newNode;
c2 := newNode;
c1↑.ntype := calcnode; (* indicate that we're a calc *)
c2↑.ntype := calcnode;
c1↑.other := f2; (* fill in other field *)
c2↑.other := f1;
c1↑.next := f1↑.calcs; (* link us to list of calcs *)
f1↑.calcs := c1;
c2↑.next := f2↑.calcs;
f2↑.calcs := c2;
c1↑.tval := nil; (* don't have a value yet *)
c2↑.tval := nil;
end;
c1↑.frame1 := true; (* say who's who *)
c2↑.frame1 := false;
c1↑.rigid := spc↑.rigid; (* remember what type of affixment *)
c2↑.rigid := spc↑.rigid;
b := ev <> nil; (* trans by var given? *)
c1↑.tvarp := b;
c2↑.tvarp := b;
if b then
begin (* indirect trans pointer *)
upTrans(ev↑.t,t); (* store away relation trans *)
c1↑.tvar := ev; (* and pointers to trans var *)
c2↑.tvar := ev;
end
else
begin (* direct trans *)
upTrans(c1↑.tval,t); (* store away relation trans *)
upTrans(c2↑.tval,t);
end;
b := false; (* assume no conflict *)
if not f1↑.ftype then (* f1 is a device *)
if not f2↑.ftype then b := f1 <> f2 (* f2 is also a device! *)
else
if f2↑.dev <> nil then b := f2↑.dev <> f1 (* f2 already dynamic *)
else affixaux(f2,f1,1) (* f2 now dynamic *)
else (* f1 is a frame *)
if not f2↑.ftype then (* f2 is a device *)
if f1↑.dev <> nil then b := f1↑.dev <> f2 (* f1 already dynamic *)
else affixaux(f1,f2,1) (* f1 now dynamic *)
else (* both frames *)
if f1↑.dev <> nil then (* f1 is dynamic *)
if f2↑.dev <> nil then b := f1↑.dev <> f2↑.dev (* both dynamic *)
else affixaux(f2,f1↑.dev,f1↑.dcntr+1) (* f2 now dynamic *)
else
if f2↑.dev <> nil then affixaux(f1,f2↑.dev,f2↑.dcntr+1); (* f1 now dynamic *)
if b then
begin
pp20L('Can''t have an affixm',20); pp20('ent chain connecting',20);
pp20(' two devices togethe',20); pp5('r! ',2); ppLine;
end;
mode := 0;
spc := spc↑.next;
end;
end;
procedure doUnfix;
var f1, f2: framep; ev: enventryp;
begin
with curInt↑ do
begin (* subscripts for frame1 & frame2 on stack *)
ev := gtVarn(spc↑.frame1); (* access variable *)
f1 := ev↑.f;
ev := gtVarn(spc↑.frame2); (* access variable *)
f2 := ev↑.f;
unfix(f1,f2); (* now unfix them *)
mode := 0;
spc := spc↑.next;
end;
end;
(* aux routines for motions: forcebits, getMechbits, moveStart, moveEnd, moveRetry *)
function forcebits(fn: nodep; var negv: boolean): integer;
var vec: vectorp; fbits: integer;
begin
fbits := XFORCE;
negv := false;
vec := nil;
with fn↑.fvec↑ do
if ntype = leafnode then vec := pcval↑.v (* first check if axis vector *)
else if op = vnegop then (* or negative axis vector *)
if arg1↑.ntype = leafnode then
begin vec := arg1↑.pcval↑.v; negv := true end;
if vec = yhat then fbits := YFORCE
else if vec = zhat then fbits := ZFORCE
else if vec <> xhat then negv := false;
if fn↑.ftype >= torque then fbits := fbits + XMOMENT;
forcebits := fbits;
end;
function getMechbits: integer;
var i: integer;
begin
with curInt↑ do
if mech = nil then i := GARMDEV (* default to green arm *)
else if mech↑.ftype then
if mech↑.dev <> nil then i := mech↑.dev↑.mech
else i := GARMDEV (* default to green arm *)
else i := mech↑.mech;
getMechbits := i;
end;
procedure moveStart;
var cl: nodep; st: statementp;
begin (* enable all cmons *)
cl := curInt↑.spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin (* check for condition monitors to enable *)
st := nil;
with cl↑ do
if ntype = cmonnode then
begin if not (cmon↑.deferCm or errHandlerp) then st := cmon end
else if ntype = viaptnode then st := vcode
else if (ntype = deprnode) or (ntype = apprnode) then st := code;
if st <> nil then
begin
with st↑.cdef↑ do
cmonEnable(getVar(level,offset)); (* enable cmon control block *)
end;
cl := cl↑.next;
end;
end;
procedure moveEnd;
var cl, val: nodep; st, err: statementp; e: enventryp; ev: eventp; fr: framep;
mechbits, errbits, angle, i: integer; errval: errortypes;
b: boolean; ch: char; kludge: interr;
begin (* disable all cmons, end of motion cleanup, error checking etc. *)
with curInt↑ do
begin
b := true;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin (* check for condition monitors to disable *)
st := nil;
with cl↑ do
if (ntype = cmonnode) and not errHandlerp then st := cmon
else if ntype = viaptnode then st := vcode
else if (ntype = deprnode) or (ntype = apprnode) then st := code;
if st <> nil then
begin
with st↑.cdef↑ do
e := getVar(level,offset); (* get cmon control block *)
if e↑.c↑.running then b := false (* is it running now? *)
else cmonDisable(e↑.c); (* if not disabled it *)
end;
cl := cl↑.next;
end;
if not b then sleep(30) (* wait for cmon's to finish *)
else
begin (* all cmon's are now done *)
if mech↑.ftype then (* get offset of device error variable *)
if mech↑.dev <> nil then i := mech↑.dev↑.vari↑.offset + 1
else i := 9 (* assume garm *)
else i := mech↑.vari↑.offset + 1;
push(newNode); (* *** for SAIL simulation version *** *)
with sp↑ do (* *** " " *** *)
begin ntype := leafnode; ltype := svaltype; s := 0.0 end; (* *** " " *** *)
errbits := round(sp↑.s); (* remember error value *)
(* Since losing Pascal doesn't have an inverse for ord *)
kludge.i := errbits div 128; (* recover error type *)
errval := kludge.err;
angle := errbits mod 128; (* also bad angles (if applicable) *)
errbits := errbits - angle; (* strip out angle info *)
setVal(0,i); (* now pop it off stack & store it away *)
err := nil;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin (* check for error checker to run *)
with cl↑ do
if (ntype = cmonnode) and errHandlerp then
begin
val := getNval(cmon↑.oncond↑.eexpr,b); (* get error bits to check *)
if errbits = round(val↑.s) then err := cmon↑.conclusion;
if b then relnode(val);
end;
cl := cl↑.next;
end;
mode := 0; (* get ready for next statement *)
if errbits <> 0 then (* was there an error? *)
if err <> nil then
begin (* run error checker *)
spc := err;
end
else
begin (* print error message *)
if mech = nil then fr := garm
else if mech↑.ftype then (* first tell what device *)
if mech↑.dev <> nil then fr := mech↑.dev
else fr := garm
else fr := mech;
with fr↑.vari↑.name↑ do prntStrng(length,name);
pp5(' - ',3);
if errval = nopower then
begin
pp20('arm power not on ',16);
ppLine;
end
else if errval = devbusy then
begin pp20('device currently in ',20); pp5('use ',4) end
else
begin
case errval of
srvdead: pp10('servo dead',10);
adcdead: pp10('a/d error ',9);
panicb: pp20('panic button pushed ',19);
exjtfc: begin pp20('excessive joint forc',20); ppChar('e'); end;
timout: pp10('time out ',8);
paslim: pp20('joint out of range ',18);
badpot: pp20('bad pot on PUMA ',15);
noarmsol: begin pp20('no arm solution whil',20); pp10('e servoing',10) end;
others: begin (* *** say unknown error type & indicate ord(errval) *** *) end;
end;
badJoints(angle); (* tell which joint(s) were bad, if any *)
end;
pp20L('"P" to proceed, "R" ',20); pp20('to retry the motion ',19);
b := (spc↑.stype <> operatetype) and (spc↑.stype <> centertype);
if b then
begin pp20(', "F" to move direct',20); pp20('ly to destination ',17) end;
pp20L(' or "B" to break to',20); pp20(' debugger: ',11);
ppOutNow;
mode := 4;
curInt↑.next := readQueue; (* *** should check that no other *)
readQueue := curInt; (* process is waiting, but... *** *)
curInt↑.status := inputqueue;
curInt := nil;
resched := true;
end
else
begin (* all ok - move on to next statement *)
spc := spc↑.next;
end
end
end;
end;
procedure moveRetry;
var ch: ascii; ev: eventp; mechbits: integer; fr: framep;
begin
with curInt↑ do
begin
mode := 0;
inputReady := false;
ch := inputLine[1]; (* what does luser want to do now? *)
if ord(ch) >= smallA then
ch := chr(ord(ch) - smallA + ord('A')); (* convert to uppercase *)
if ch = 'B' then running := false (* break to debugger, proceed will retry *)
else if ch = 'P' then spc := spc↑.next (* move on to next statement *)
(* else if ch = 'R' then nothing to do *)
else if (ch = 'F') and
(spc↑.stype <> operatetype) and (spc↑.stype <> centertype) then
begin
mode := 3;
ev := getEvent; (* event to use when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := getMechbits;
with msg↑ do
begin
cmd := movehdrcmd;
dev := mechbits;
bits := NULLINGCB + DURLBCB; (* nonulling & duration *)
evt := ev;
dur := 5.0; (* default time of 5 seconds *)
sfac := 1.0;
if mech = nil then fr := garm
else if mech↑.ftype then
if mech↑.dev <> nil then fr := mech↑.dev
else fr := garm
else fr := mech;
if spc↑.stype = movetype then
begin
n := 1; (* only one segment *)
sendCmd; (* send over move header *)
cmd := movesegcmd;
bits := DESTPTCB;
sendTrans(fr↑.tdest); (* send over destination point *)
end
else
begin
pos := fr↑.sdest;
if pos < 0.0 then
begin (* no dest specified *)
pos := 0.0;
if spc↑.stype = opentype then bits := 3 else bits := 1;
(* *** need to set DURLBCB too??? *** *)
end
else
bits := bits + DESTPTCB; (* indicate specifying opening *)
if mechbits = VISEDEV then
begin
cmd := operatecmd; (* vise uses an operate command *)
v2 := 0.0; (* no stop wait time *)
end;
sendCmd;
end;
end;
(* if mechbits <> VISEDEV then signalArm; (* start it up *)
(* curInt↑.status := devicewait; (* don't for simulation version *)
(* curInt := nil;
(* resched := true; (* swap someone else in *)
freeEvent(ev); (* sim ver *)
end;
end;
end;
procedure doCmon;
var e: enventryp; n: nodep; b: boolean; val: nodep; r: real; fbits,i: integer;
sst: statementp;
begin
with curInt↑ do
case mode of
1: begin
if not spc↑.deferCm then (* check it's not a deferred cmon *)
begin (* need to enable the cmon *)
with spc↑.cdef↑ do
cmonEnable(getEntry(level,offset)); (* enable cmon control block *)
end;
mode := 0;
spc := spc↑.next;
end;
2: begin (* deal with ON condition *)
n := nil;
mode := 3; (* set up for doing conclusion next time *)
if spc↑.exprCm then
begin (* test if expression is now true *)
n := pop; (* get expression value *)
if n↑.s = 0.0 then
begin
sleep(20); (* no good - try again in 0.33 seconds *)
mode := 0;
end;
end
else if spc↑.oncond↑.ntype = durnode then
begin (* duration cmon *)
n := pop;
sleep(round(n↑.s * 60)); (* get wait time (in 60Hz ticks) *)
end
else if spc↑.oncond↑.ntype = forcenode then
begin (* force sensing *)
val := getNval(spc↑.oncond↑.fval,b); (* get force magnitude *)
r := val↑.s;
if b then relNode(val);
fbits := forcebits(spc↑.oncond,b);
with spc↑.oncond↑ do
begin
if (ftype = absforce) or (ftype = abstorque) then fbits := fbits + SIGMAG;
if b then begin r := -r; if frel < seqop then fbits := fbits + SIGGE end
else if frel >= seqop then fbits := fbits + SIGGE;
end;
with spc↑.conclusion↑ do
if stype = stoptype then
begin (* set FSTOP bit if no explicit frame is being stopped *)
if cf = nil then fbits := fbits + FSTOP
else if cf↑.ntype = leafnode then
begin (* need to check if same device as current mech *)
e := gtVarn(cf); (* get variable frame *)
if e↑.etype = frametype then
begin
if e↑.f = nil then i := GARMDEV (* default to green arm *)
else with e↑.f↑ do
if ftype then
if dev <> nil then i := dev↑.mech
else i := GARMDEV (* default to green arm *)
else i := mech;
if i = getMechBits then fbits := fbits + FSTOP;
end
end;
(* ** can't check if array ref since subscripts aren't on stack ** *)
end
else if stype = blocktype then
if bcode↑.stype = stoptype then
if bcode↑.cf = nil then fbits := fbits + FSTOP;
cm↑.fbits := fbits; (* remember bits in cmoncb *)
with msg↑ do
begin
cmd := forcesigcmd;
dev := getMechbits; (* deal with which arm here *)
bits := fbits;
evt := cm↑.evt;
mag := r;
end;
sendCmd;
cm↑.evt↑.count := -1;
cm↑.evt↑.waitlist := curInt; (* put us on event waitlist *)
curInt↑.status := forcewait;
curInt := nil; (* swap in someone else *)
resched := true;
end
else if spc↑.oncond↑.ntype = departingnode then
begin (* departing cmon *)
sleep(30); (* wait 0.5 seconds (in 60Hz ticks) *)
end
else
begin (* event cmon *)
if spc↑.oncond↑.ntype = arrivalnode then
with spc↑.oncond↑.evar↑ do e := getVar(level,offset)
else e := gtVarn(spc↑.oncond);
cm↑.evt := e↑.evt; (* save pointer to event we're waiting on *)
e↑.evt↑.count := e↑.evt↑.count - 1;
if e↑.evt↑.count <= 0 then (* hasn't been signalled yet, need to wait *)
begin
addPdb(e↑.evt↑.waitlist,curInt); (* add us to wait list *)
curInt↑.status := eventqueue;
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
if n <> nil then relNode(n);
end;
3: begin
mode := 0;
if cm↑.enabled then (* check that we're still enabled *)
begin
cm↑.running := true; (* set up current cmon status *)
cm↑.enabled := false;
spc := spc↑.conclusion;
end
else
begin
curInt↑.status := nullqueue;
curInt := nil; (* we should go away *)
resched := true; (* now swap in highest priority process *)
end;
end;
end;
end;
procedure doMove;
var appr,depr,dest,arrv,wobble,sfac,dur,ffr,stiff,gather,zwrist,n: nodep;
cl,val,val1,val2: nodep; t,tl,tb: transp; st: statementp; e: enventryp;
r: real; fbits,nsegs,mechbits,i,cmForce,useForce: integer; fr: framep;
b,b1,b2,nulling,apprp,deprp: boolean; ev: eventp;
function getLoc(n: nodep): transp;
var tp: transp; b: boolean;
begin
n := getNval(n,b);
tp := n↑.t;
if b then relnode(n);
if t <> nil then tp := ttmul(t,tp);
getLoc := tp;
end;
function getDepr(n: nodep; b: boolean): transp;
var tp: transp; v: vectorp;
begin
if n↑.ltype = svaltype then tp := tmake(niltrans,svmul(n↑.s,zhat))
else if n↑.ltype = vectype then tp := tmake(niltrans,n↑.v)
else tp := n↑.t;
if b then relnode(n);
tp := ttmul(tb,tp); (* shift to proper coord sys *)
if t <> nil then tp := ttmul(t,tp);
getDepr := tp;
end;
procedure getCode(s: statementp);
var e: enventryp;
begin
if s = nil then e := nil
else
begin
with s↑ do
if stype = signaltype then e := gtVarn(event)
else e := gtVarn(oncond);
msg↑.evt := e↑.evt; (* event to signal for code *)
msg↑.bits := msg↑.bits + CODECB;
end;
end;
begin
with curInt↑ do
begin
st := spc; (* remember MOVE statement *)
case mode of
1: begin (* set up force system, enable all cmons *)
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
mechbits := getMechbits;
if mech↑.ftype then (* check it's a device *)
if mech↑.dev = nil then
begin (* yow! frame that's not affixed to an arm *)
pp20L('Control frame not af',20); pp20('fixed to any device:',20);
pp20(' Assuming garm ',14); ppLine;
end;
ffr := nil;
stiff := nil;
gather := nil;
zwrist := nil;
cmForce := 0;
useForce := 0;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
if ntype = ffnode then ffr := cl
else if ntype = stiffnode then stiff := cl
else if ntype = gathernode then gather := cl
else if ntype = wristnode then zwrist := cl
else if ntype = forcenode then useForce := useForce + 1
else if ntype = cmonnode then
if cmon↑.oncond↑.ntype = forcenode then cmForce := cmForce + 1;
cl := next;
end;
if (ffr <> nil) or (cmForce + useForce > 0) or (gather <> nil) then
begin
msg↑.cmd := setccmd;
msg↑.dev := mechbits; (* tell which arm *)
msg↑.bits := FTABLE; (* assume this *)
if ffr <> nil then
begin
val := getNval(ffr↑.ff,b); (* get force frame value *)
if not ffr↑.csys then msg↑.bits := 0;
sendTrans(val↑.t); (* send command & trans over *)
if b then relNode(val);
end
else sendTrans(niltrans); (* send command & trans over *)
(* signalArm; (* wake up ARM servo background job *)
end;
if zwrist <> nil then b := not zwrist↑.notp
else b := (ffr <> nil) or (stiff <> nil) or (cmForce > 0);
if b then
begin
msg↑.cmd := zerowristcmd; (* tell arm servo to zero wrist *)
msg↑.dev := mechbits; (* tell which wrist *)
sendCmd;
end;
if stiff <> nil then
begin
val1 := getNval(stiff↑.fv,b1); (* get force vector *)
val2 := getNval(stiff↑.mv,b2); (* get moment vector *)
if stiff↑.coc <> nil then
begin
val := getNval(stiff↑.coc,b); (* get coc value *)
t := val↑.t;
end
else begin t := niltrans; b := false end;
with msg↑ do
begin
cmd := setstiffcmd;
(* *** dev := mechbits; (* tell which arm? *** *)
for i := 1 to 3 do
begin
t[i] := val1↑.v↑.val[i];
t[i+3] := val2↑.v↑.val[i];
end;
end;
sendTrans(t); (* send stiffnesses & coc trans over *)
(* signalArm; (* wake up ARM servo background job *)
if b1 then killNode(val1);
if b2 then killNode(val2);
if b then relNode(val);
end
else if useForce > 0 then
begin (* add default stiffness *)
with msg↑ do
begin
cmd := setstiffcmd;
(* *** dev := mechbits; (* tell which arm? *** *)
for i := 1 to 3 do
begin
t[i] := 40;
t[i+3] := 100;
end;
end;
sendTrans(niltrans); (* send stiffnesses & coc trans over *)
(* signalArm; (* wake up ARM servo background job *)
end;
if gather <> nil then
begin
with msg↑ do
begin
cmd := gathercmd;
dev := mechbits; (* tell with which arm *)
bits := gather↑.gbits;
end;
sendCmd; (* send gather command over *)
end;
if ffr <> nil then (* no bias forces if no force frame *)
begin
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin
with cl↑ do
if ntype = forcenode then (* check for bias forces *)
begin
val := getNval(cl↑.fval,b); (* get force magnitude *)
r := val↑.s;
if b then relnode(val);
fbits := forcebits(cl,b);
if b then r := -r;
with msg↑ do
begin
cmd := biasoncmd;
dev := mechbits; (* tell with which arm *)
bits := fbits;
mag := r;
end;
sendCmd; (* tell arm about bias force *)
end;
cl := cl↑.next;
end;
end;
moveStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := getMechbits;
nsegs := 0;
if mech↑.ftype then
if mech↑.dev <> nil then fr := mech↑.dev (* get frame for device *)
else fr := garm
else fr := mech;
nulling := true; (* no nulling is the default *)
dest := nil;
wobble := nil;
sfac := nil;
dur := nil;
arrv := nil;
appr := nil;
depr := nil;
apprp := true; (* assume default approach *)
deprp := fr↑.depr <> nil; (* default departure if last had approach *)
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
if ntype = destnode then begin dest := cl; nsegs := nsegs + 1 end
else if ntype = wobblenode then wobble := cl
else if ntype = sfacnode then sfac := cl
else if ntype = durnode then dur := cl
else if ntype = nullingnode then nulling := notp
else if ntype = apprnode then
begin
appr := cl;
if loc = nil then apprp := false (* approach = nildeproach *)
else begin apprp := true; nsegs := nsegs + 1 end
end
else if ntype = deprnode then
begin
depr := cl;
if loc = nil then deprp := false (* departure = nildeproach *)
else begin deprp := true; nsegs := nsegs + 1 end
end
else if ntype = viaptnode then nsegs := nsegs + 1
else if ntype = cmonnode then
begin
if cmon↑.oncond↑.ntype = arrivalnode then arrv := cmon↑.oncond;
end;
cl := next;
end;
if deprp or mech↑.ftype then
tb := feval(mech); (* get current cf position *)
if deprp then
if depr <> nil then (* explicit departure point? *)
tb↑.refcnt := tb↑.refcnt + 1 (* need it to compute departure *)
else nsegs := nsegs + 1; (* add in default departure seg *)
if apprp and (appr = nil) then (* default approach point? *)
with dest↑.loc↑ do
if ((ntype = leafnode) and (ltype = varitype)) or
((ntype = exprnode) and (op = arefop)) then
nsegs := nsegs + 1 (* add in default approach seg *)
else apprp := false; (* don't want default approach *)
if mech↑.ftype then
begin (* get offset trans to take cf to arm *)
t := whereArm(mechbits); (* Get current device pos *)
t := ttmul(t,tinvrt(tb)); (* compute offset *)
end
else t := nil; (* no offset needed *)
with msg↑ do
begin
cmd := movehdrcmd;
dev := mechbits;
if nulling then bits := NULLINGCB else bits := 0;
n := nsegs;
evt := ev;
end;
if sfac <> nil then
begin (* use local speed factor *)
val := getNval(sfac↑.clval,b);
msg↑.sfac := val↑.s;
if b then relnode(val);
end
else
begin (* use global speed factor *)
msg↑.sfac := speedfactor↑.s;
end;
if dur <> nil then (* duration *)
begin
val := getNval(dur↑.durval,b);
msg↑.dur := val↑.s;
if dur↑.durrel < seqop then i := DURLBCB
else if dur↑.durrel > seqop then i := DURUBCB
else i := DUREQCB;
msg↑.bits := msg↑.bits + i;
if b then relnode(val);
end;
if wobble <> nil then (* wobble *)
begin
val := getNval(wobble↑.clval,b);
msg↑.wobble := val↑.s;
msg↑.bits := msg↑.bits + WOBBLECB;
if b then relnode(val);
end;
sendCmd; (* tell arm servo we're starting a motion *)
msg↑.cmd := movesegcmd; (* now get values for trajectory points *)
if deprp then (* departure: loc & event *)
begin
msg↑.bits := DEPRPTCB;
if depr = nil then tl := fr↑.depr (* default departure point *)
else
begin (* explicit departure point *)
n := getNval(depr↑.loc,b);
tl := getDepr(n,b);
tb↑.refcnt := tb↑.refcnt - 1;
if tb↑.refcnt <= 0 then relTrans(tb); (* done with it now *)
getCode(depr↑.code);
end;
sendTrans(tl);
end;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin
with cl↑ do
if ntype = viaptnode then (* vias: loc, duration, velocity & event *)
begin
msg↑.bits := VIAPTCB;
tl := getLoc(via);
if duration <> nil then
begin
val := getNval(duration↑.durval,b);
msg↑.dur := val↑.s;
if duration↑.durrel < seqop then i := DURLBCB
else if duration↑.durrel > seqop then i := DURUBCB
else i := DUREQCB;
msg↑.bits := msg↑.bits + i;
if b then relnode(val);
end;
if velocity <> nil then
begin
val := getNval(velocity,b);
msg↑.bits := msg↑.bits + VELOCCB;
with val↑.v↑ do
begin
msg↑.v1 := val[1];
msg↑.v2 := val[2];
msg↑.v3 := val[3];
end;
if b then relnode(val);
end;
getCode(cl↑.vcode);
sendTrans(tl);
end;
cl := cl↑.next;
end;
if apprp then (* approach: loc & event *)
begin
msg↑.bits := APPRPTCB;
if appr <> nil then
begin (* explicit approach point *)
n := getNval(appr↑.loc,b);
getCode(appr↑.code);
end;
tb := getLoc(dest↑.loc); (* need to get destination location *)
tb↑.refcnt := tb↑.refcnt + 1; (* make sure we keep it for later *)
if appr <> nil then tl := getDepr(n,b) (* explicit approach point *)
else
begin (* default appoach point *)
tl := tvadd(tb,svmul(3,zhat));
if t <> nil then tl := ttmul(t,tl);
end;
tb↑.refcnt := tb↑.refcnt - 1;
upTrans(fr↑.appr,tl); (* save it for next motion *)
sendTrans(tl);
end
else
begin
tb := getLoc(dest↑.loc); (* get destination for below *)
upTrans(fr↑.appr,nil); (* remember no default depr for next motion *)
end;
(* destination: loc & event *)
uptrans(fr↑.tdest,tb); (* make a copy of dest for later use *)
msg↑.bits := DESTPTCB;
if arrv <> nil then
begin
with arrv↑.evar↑ do e := getVar(level,offset);
msg↑.evt := e↑.evt; (* event to signal for code *)
msg↑.bits := msg↑.bits + CODECB;
end;
sendTrans(tb);
(* signalArm; (* finally let background job deal with traj *)
mode := 3;
(* curInt↑.status := devicewait; (* don't for simulation version *)
(* curInt := nil;
(* resched := true; (* swap someone else in *)
freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
if curInt <> nil then (* in case we're waiting for an error response *)
if spc = st↑.next then
begin (* doesn't appear to have been any errors *)
if mech↑.ftype then (* get frame for device *)
if mech↑.dev <> nil then fr := mech↑.dev
else fr := garm
else fr := mech;
upTrans(fr↑.depr,fr↑.appr); (* update default departure point *)
end;
end;
end;
procedure doOperate;
var durcl,vel,torquecl,cl,v: nodep; e: enventryp; b,ccw: boolean; ev: eventp;
begin (* deal with driver *)
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
moveStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
durcl := nil;
vel := nil;
torquecl := nil;
ccw := false;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
if ntype = durnode then durcl := cl
else if ntype = forcenode then
begin
if ftype = torque then torquecl := cl
else if ftype = angvelocity then vel := cl
end
else if ntype = cwnode then ccw := notp;
cl := next;
end;
with msg↑ do
begin
cmd := operatecmd;
dev := getMechbits;
bits := 0;
evt := ev;
dur := 5.0; (* default values *)
v1 := 60.0; (* angular velocity *)
v2 := 0.0; (* torque *)
if durcl <> nil then
begin
v := getNval(durcl↑.durval,b); (* get duration value *)
dur := v↑.s;
if b then relNode(v);
end;
if vel <> nil then
begin
v := getNval(vel↑.fval,b); (* get angular velocity value *)
v1 := v↑.s;
if b then relNode(v);
end;
if torquecl <> nil then
begin
v := getNval(torquecl↑.fval,b); (* get torque value *)
v2 := v↑.s;
if b then relNode(v);
end;
if ccw then
begin (* turning counterclockwise *)
v1 := - v1;
v2 := - v2;
end;
end;
sendCmd; (* pass info to ARM servo *)
mode := 3;
(* curInt↑.status := devicewait; (* don't for simulation version *)
(* curInt := nil;
(* resched := true; (* swap someone else in *)
freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
end;
procedure doOpen; (* & doClose *)
var dest,sfac,durcl,swt,cl,v: nodep; e: enventryp; ev: eventp;
opening,dtime,sf,swtime: real; mechbits: integer; b,nulling: boolean;
begin
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
moveStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := getMechbits;
(* run through clauses for dest, duration & speed factor/stop wait time specs *)
dest := nil;
durcl := nil;
sfac := nil;
swt := nil;
nulling := true; (* nonulling is the default *)
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
if ntype = destnode then dest := cl
else if ntype = durnode then durcl := cl
else if ntype = sfacnode then sfac := cl
else if ntype = swtnode then swt := cl
else if ntype = nullingnode then nulling := notp;
cl := next;
end;
if sfac = nil then sf := speedfactor↑.s (* use global speed factor *)
else
begin
v := getNval(sfac↑.clval,b); (* get local speed factor value *)
sf := v↑.s;
if b then relNode(v);
end;
if durcl = nil then dtime := 0
else
begin
v := getNval(durcl↑.durval,b); (* get duration value *)
dtime := v↑.s;
if b then relNode(v);
end;
if swt = nil then swtime := 0
else
begin
v := getNval(swt↑.clval,b); (* get stop wait time value *)
swtime := v↑.s;
if b then relNode(v);
end;
if dest = nil then
begin
opening := 0;
mech↑.sdest := -1; (* so we know there was no dest *)
end
else
begin
v := getNval(dest↑.loc,b); (* get opening value *)
opening := v↑.s;
mech↑.sdest := opening; (* remember it *)
if b then relNode(v);
end;
with msg↑ do
begin
dev := mechbits;
evt := ev;
if nulling then bits := NULLINGCB else bits := 0;
if dest = nil then
begin
pos := 0.0;
if spc↑.stype = opentype then bits := 3 else bits := 1;
end
else
begin
pos := opening;
bits := bits + DESTPTCB; (* indicate we're specifying opening *)
end;
if durcl = nil then dur := 0.0
else
begin
dur := dtime;
bits := bits + DUREQCB;
end;
sfac := sf;
if mechbits = VISEDEV then
begin
cmd := operatecmd; (* vise uses an operate command *)
if swt = nil then
if dest = nil then v2 := 0.25 else v2 := 0.0 (* default values *)
else v2 := swtime;
if durcl = nil then dur := 8.0;
sendCmd;
end
else
begin
cmd := movehdrcmd; (* deal with hand *)
sendCmd;
(* signalArm; (* since movehdr normally followed by movesegs *)
end;
end;
mode := 3;
(* curInt↑.status := devicewait; (* don't for simulation version *)
(* curInt := nil;
(* resched := true; (* swap someone else in *)
freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
end;
procedure doCenter;
var e: enventryp; ev: eventp;
begin
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
moveStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
with msg↑ do
begin
cmd := centercmd;
dev := getMechbits;
bits := 0;
evt := ev;
end;
sendCmd; (* initiate the center operation *)
mode := 3;
(* curInt↑.status := devicewait; (* don't for simulation version *)
(* curInt := nil;
(* resched := true; (* swap someone else in *)
freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
end;
procedure doStop;
var mechbits: integer; e: enventryp;
begin
with curInt↑ do
begin
if spc↑.cf = nil then mechbits := getMechbits (* use current mech *)
else
begin
e := gtVarn(spc↑.cf); (* see what we're stopping *)
with e↑.f↑ do
if ftype then
if dev <> nil then mechbits := dev↑.mech
else
begin (* yow! frame that's not affixed to a device *)
pp20L('Attempt to stop fram',20); pp20('e not affixed to any',20);
pp20(' device: Assuming ga',20); pp5('rm ',2); ppLine;
mechbits := GARMDEV;
end
else mechbits := mech;
end;
with msg↑ do
begin
cmd := stopcmd;
dev := mechbits;
end;
sendCmd; (* tell arm servo to stop device *)
mode := 0;
spc := spc↑.next;
end;
end;
procedure doRetry;
var b: boolean;
begin
with curInt↑ do
begin
if spc↑.rparent <> nil then
begin
b := true;
while b and (spc↑.olevel < getELev(env)) do
begin (* make sure all cmon's in outer environments have finished *)
b := cmonCheck;
if b then killEnv; (* flush all environments out to move *)
end;
if b then (* no cmons now running *)
begin
(* *** might need to clean up stack some here (fornodes) *** *)
spc := spc↑.rcode; (* go redo the previous motion *)
mode := 0;
end
else sleep(30); (* give cmons time to finish *)
end
else
begin
spc := spc↑.next; (* just go on to next statement *)
mode := 0;
end;
end;
end;
procedure doSetbase;
begin
with curInt↑ do
begin
msg↑.cmd := zerowristcmd; (* tell ARM servo to zero wrist *)
sendCmd;
mode := 0;
spc := spc↑.next;
end;
end;
procedure doWrist;
var fv,tv: enventryp; v: vectorp; i: integer;
begin
with curInt↑ do
begin
fv := gtVarn(spc↑.fvec); (* get where to store results *)
tv := gtVarn(spc↑.tvec);
if fv↑.v <> nil then (* flush any old values *)
with fv↑.v↑ do
begin
refcnt := refcnt - 1;
if refcnt <= 0 then relVector(fv↑.v);
end;
if tv↑.v <> nil then
with tv↑.v↑ do
begin
refcnt := refcnt - 1;
if refcnt <= 0 then relVector(tv↑.v);
end;
msg↑.cmd := wristcmd;
getReply; (* have ARM servo read wrist *)
v := newVector;
for i := 1 to 3 do v↑.val[i] := msg↑.t[i];
fv↑.v := v; (* store away force vector *)
v↑.refcnt := 1;
v := newVector;
for i := 1 to 3 do v↑.val[i] := msg↑.t[i+3];
tv↑.v := v; (* store away torque vector *)
v↑.refcnt := 1;
mode := 0;
spc := spc↑.next;
end;
end;
(* command loop *)
procedure interp(dLev: integer);
var p,pp: pdbp; n: nodep; b,breakNow: boolean; ch: ascii; minPriority: integer;
begin
debugLevel := dLev;
minPriority := 10 * debugLevel;
if curInt <> nil then curInt↑.status := nowrunning;
running := true;
resched := false;
breakNow := false;
escapeI := false;
inputp := 0;
inputReady := false;
if readQueue <> nil then
if readQueue↑.priority >= minPriority then (* must be at current level *)
with readQueue↑ do
begin (* remind user we're waiting for input *)
b := true;
if epc <> nil then
begin
b := false;
if epc↑.op = queryop then pp20L('Type Y or N: ',13)
else if epc↑.op = inscalarop then pp20L('Scalar please: ',15)
else b := true;
end;
if b then
begin
b := false;
if (spc↑.stype = prompttype) or (spc↑.stype = waittype) then
pp20L('Type P to proceed: ',19)
else if (movetype <= spc↑.stype) and (spc↑.stype <= centertype) then
begin
pp20L('"P" to proceed, "R" ',20); pp20('to retry the motion ',19);
if (spc↑.stype <> operatetype) and (spc↑.stype <> centertype) then
begin pp20(', "F" to move direct',20);
pp20('ly to destination ',17) end;
pp20L(' or B to break to d',20); pp10('ebugger: ',9);
end
else b := true;
end;
if not b then ppOutNow;
(* *** else ??? flush readQueue ??? *** *)
end;
while running do
begin
if msgp then (* any messages pending? *)
repeat (* yup - go read them *)
msgp := false; (* reset flag *)
b := getArm; (* read next message *)
if b then msgDispatch (* if we actually got one then deal with it *)
until not b; (* keep going til no more messages to read *)
if stime <> 0 then (* hack on 10 to simulate time *)
begin
stime := stime - 1;
if stime = 0 then (* time to wake up sleeping processes *)
begin
n := clkQueue; (* get waitlist node *)
clkQueue := n↑.next;
if clkQueue <> nil then stime := clkQueue↑.when; (* set stime for next *)
p := n↑.who;
while p <> nil do (* add waiting processes to activeInts list *)
begin
pp := p↑.next; (* remember where we are in list *)
p↑.status := runqueue;
addPdb(activeInts,p);
p := pp;
end;
relNode(n);
if curInt = nil then resched := true
else if activeInts↑.priority > curInt↑.priority then resched := true;
end;
end;
if resched then (* schedule highest priority process *)
begin
resched := false;
if curInt <> nil then
begin
curInt↑.status := runqueue;
addPdb(activeInts,curInt);
end;
curInt := activeInts; (* now swap in highest priority process *)
if activeInts <> nil then
begin
activeInts := activeInts↑.next;
curInt↑.next := nil;
curInt↑.status := nowrunning;
with curInt↑ do
breakNow := (mode = 0) and (spc↑.bpt or spc↑.bad);
end;
end;
if readQueue <> nil then (* is some process waiting for terminal input? *)
if readQueue↑.priority >= minPriority then (* must be at current level *)
while anyChar(ch) and (not inputReady) do
begin
if ch = chr(CR) then
begin (* process the line now *)
ppLine; (* echo it *)
inputReady := true;
if inputp = 0 then inputLine[1] := ' '; (* for empty lines *)
if curInt <> nil then
begin
curInt↑.status := runqueue;
curInt↑.next := activeInts;
activeInts := curInt;
resched := curInt↑.priority > readQueue↑.priority; (* for next time *)
end;
curInt := readQueue; (* swap input process in now *)
curInt↑.status := nowrunning;
readQueue := curInt↑.next; (* might be a lower level joker in queue *)
curInt↑.next := nil;
breakNow := false;
end
else if (ord(ch) = ctlH) or (ord(ch) = deletekey) then (* backspace/delete *)
begin
if inputp > 0 then
begin (* something to delete *)
inputLine[inputp] := ' ';
inputp := inputp - 1;
ppDelChar; (* erase last character *)
end
end
else if ch <> chr(LF) then (* ignore linefeeds *)
begin
inputp := inputp + 1; (* *** should check for array overflow *** *)
inputLine[inputp] := ch;
ppChar(ch); ppOutNow; (* echo it *)
end
end;
if (curInt <> nil) and (not breakNow) then (* something to do now *)
with curInt↑ do
if priority >= minPriority then (* must be at current level *)
if epc <> nil then evalExp (* continue evaluating current expression *)
else if curInt↑.mode = 0 then
begin (* evaluate any expressions needed by current statement *)
epc := spc↑.exprs;
mode := 1;
if spc↑.stype = untiltype then epc := nil (* evaluate condition later *)
else if spc↑.stype = cmtype then (* treat enabling a cmon specially *)
if cm = nil then epc := nil
else if cm↑.cmon <> spc then epc := nil
else mode := 2; (* we're doing the ON cond *)
end
else case spc↑.stype of (* interpret the current statement *)
progtype: doProg;
blocktype: doBlock;
coblocktype: doCoblock;
coendtype,
endtype: doEnd;
fortype: doFor;
iftype: doIf;
whiletype: doWhile;
untiltype: doUntil;
casetype: doCase;
calltype: doCall;
returntype: doReturn;
printtype: doPrint;
prompttype: doPrompt;
pausetype: doPause;
aborttype: doAbort;
assigntype: doAssign;
signaltype: doSignal;
waittype: doWait;
enabletype: doEnable;
disabletype: doDisable;
cmtype: doCmon;
affixtype: doAffix;
unfixtype: doUnfix;
movetype: doMove;
operatetype: doOperate;
opentype,
closetype: doOpen; (* someday close may be different ... *)
centertype: doCenter;
stoptype: doStop;
retrytype: doRetry;
setbasetype: doSetbase;
wristtype: doWrist;
evaltype,
commenttype,
emptytype,
requiretype,
definetype,
declaretype,
dimdeftype: begin
if spc↑.stype = evaltype then
spc↑.aval := pop; (* get value for EDIT *)
mode := 0;
spc := spc↑.next; (* move on *)
end;
(* more??? *)
end;
if (curInt <> nil) and running then (* check if we've hit a breakpoint *)
with curInt↑ do
if priority >= minPriority then (* must be at current level *)
running := not((mode = 0) and (spc↑.bpt or spc↑.bad));
if escapeI then
begin
b := running;
if curInt = nil then running := false
else with curInt↑ do
if priority < minPriority then running := false
else if curInt↑.mode = 0 then (* ready to start some real stmnt? *)
if (spc↑.stype <> endtype) and (spc↑.stype <> coendtype) then
running := false;
if b and not running then pp20L('Escape-I interrupt ',18);
end;
end; (* repeat til done running *)
(* finish up - leave things in a clean state *)
end;
begin
end.